diff options
46 files changed, 4046 insertions, 4582 deletions
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg Binary files differindex 29d738c39..1ef05458f 100644 --- a/Documentation/rc_mode_switch.odg +++ b/Documentation/rc_mode_switch.odg diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf Binary files differindex 856dd55c5..b1a468d17 100644 --- a/Documentation/rc_mode_switch.pdf +++ b/Documentation/rc_mode_switch.pdf diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec..9aca3fc5f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -fw_att_pos_estimator start +ekf_att_pos_estimator start # # Start attitude controller diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index ed3939757..268eb9bba 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,6 +5,7 @@ # attitude_estimator_ekf start +#ekf_att_pos_estimator start position_estimator_inav start mc_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a..c1f9db7d1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -35,6 +35,12 @@ then param set MPC_TILT_MAX 1.0 param set MPC_LAND_SPEED 1.0 param set MPC_LAND_TILT 0.3 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELNE_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + fi set PWM_RATE 400 diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d0..1daf8277e 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,7 +70,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav #MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc..7f0c59515 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfcf91e3f..57a03bc84 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.global_valid) { + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f72dc607c..9a24ff50e 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +39,7 @@ * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <geo/geo.h> @@ -52,124 +50,58 @@ #include <math.h> #include <stdbool.h> +/* + * Azimuthal Equidistant Projection + * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html + */ -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; + ref->lat = lat_0 / 180.0 * M_PI; + ref->lon = lon_0 / 180.0 * M_PI; + ref->sin_lat = sin(ref->lat); + ref->cos_lat = cos(ref->lat); } -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + double lat_rad = lat / 180.0 * M_PI; + double lon_rad = lon / 180.0 * M_PI; - if (0 != c) - k_bar = c / sin(c); + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon); - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (c == 0.0) ? 1.0 : (c / sin(c)); -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; } -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + double lat_rad; + double lon_rad; - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + if (c != 0.0) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } + lat_rad = ref->lat; + lon_rad = ref->lon; } -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; } @@ -207,7 +139,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -222,7 +154,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -248,7 +180,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -265,7 +197,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -296,8 +228,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and @@ -316,29 +248,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } if (arc_sweep >= 0) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; - if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; - if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } } in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -394,8 +326,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d } __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, - double lat_next, double lon_next, float alt_next, - float *dist_xy, float *dist_z) + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) { double current_x_rad = lat_next / 180.0 * M_PI; double current_y_rad = lon_next / 180.0 * M_PI; @@ -419,8 +351,8 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, - float x_next, float y_next, float z_next, - float *dist_xy, float *dist_z) + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) { float dx = x_now - x_next; float dy = y_now - y_next; @@ -442,15 +374,19 @@ __EXPORT float _wrap_pi(float bearing) int c = 0; while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; while (bearing < -M_PI_F) { bearing += M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -466,15 +402,19 @@ __EXPORT float _wrap_2pi(float bearing) int c = 0; while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; while (bearing < 0.0f) { bearing += M_TWOPI_F; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -490,15 +430,19 @@ __EXPORT float _wrap_180(float bearing) int c = 0; while (bearing >= 180.0f) { bearing -= 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; while (bearing < -180.0f) { bearing += 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; @@ -514,15 +458,19 @@ __EXPORT float _wrap_360(float bearing) int c = 0; while (bearing >= 360.0f) { bearing -= 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } c = 0; while (bearing < 0.0f) { bearing += 360.0f; - if (c++ > 3) + + if (c++ > 3) { return NAN; + } } return bearing; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 94afb4df0..0a3f85d97 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +39,7 @@ * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ @@ -67,6 +65,14 @@ struct crosstrack_error_s { float bearing; // Bearing in radians to closest point on line/arc } ; +/* lat/lon are in radians */ +struct map_projection_reference_s { + double lat; + double lon; + double sin_lat; + double cos_lat; +}; + /** * Initializes the map transformation. * @@ -74,7 +80,7 @@ struct crosstrack_error_s { * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(double lat_0, double lon_0); +__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); /** * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane @@ -83,7 +89,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0); * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); +__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); /** * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system @@ -93,7 +99,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y) * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); +__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); /** * Returns the distance to the next waypoint in meters. @@ -115,30 +121,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); /* * Calculate distance in global frame */ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, - double lat_next, double lon_next, float alt_next, - float *dist_xy, float *dist_z); + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); /* * Calculate distance in local frame (NED) */ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, - float x_next, float y_next, float z_next, - float *dist_xy, float *dist_z); + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 668bac5d9..000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,815 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include <poll.h> - -#include "KalmanNav.hpp" -#include <systemlib/err.h> -#include <geo/geo.h> - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index caf93bc78..000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include <nuttx/config.h> - -#include <mathlib/mathlib.h> -#include <controllib/blocks.hpp> -#include <controllib/block/BlockParam.hpp> -#include <uORB/Subscription.hpp> -#include <uORB/Publication.hpp> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/parameter_update.h> - -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> - -#include <drivers/drv_hrt.h> -#include <poll.h> -#include <unistd.h> - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */ - uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */ - uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */ - // publications - uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */ - uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */ - uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d..000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <systemlib/systemlib.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> -#include <drivers/drv_hrt.h> -#include <math.h> -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d95..000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edead..000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include <systemlib/param/param.h> - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 10a6cd2c5..c61b6ff3f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f4b24cff1..352711734 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> @@ -112,12 +113,11 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -153,6 +153,7 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; +static float eph_epv_threshold = 5.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -194,7 +195,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -207,7 +208,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -378,7 +379,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -536,6 +537,51 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_DO_SET_HOME: { + bool use_current = cmd->param1 > 0.5f; + if (use_current) { + /* use current position */ + if (status->condition_global_position_valid) { + home->lat = global_pos->lat; + home->lon = global_pos->lon; + home->alt = global_pos->alt; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* use specified position */ + home->lat = cmd->param5; + home->lon = cmd->param6; + home->alt = cmd->param7; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + + /* announce new home position */ + if (*home_pub > 0) { + orb_publish(ORB_ID(home_position), *home_pub, home); + + } else { + *home_pub = orb_advertise(ORB_ID(home_position), home); + } + + /* mark home position as set */ + status->condition_home_position_valid = true; + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -568,6 +614,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; + bool was_armed = false; /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); @@ -771,6 +818,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -876,7 +928,45 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); + /* hysteresis for EPH/EPV */ + bool eph_epv_good; + if (status.condition_global_position_valid) { + if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + eph_epv_good = false; + } + + } else { + if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { + eph_epv_good = true; + } + } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + + /* check if GPS fix is ok */ + + /* update home position */ + if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; + tune_positive(true); + } /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -887,7 +977,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; @@ -962,6 +1052,13 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update position setpoint triplet */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -1024,45 +1121,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* check if GPS fix is ok */ - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { - - /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } } /* start RC input check */ @@ -1092,7 +1150,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1110,7 +1168,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1150,11 +1208,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { @@ -1165,6 +1220,33 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + /* MISSION switch */ + if (sp_man.loiter_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1189,10 +1271,15 @@ int commander_thread_main(int argc, char *argv[]) } else { /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + transition_result_t res = TRANSITION_DENIED; + + if (!status.condition_landed) { + /* vehicle is not landed, try to perform RTL */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + } if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ + /* RTL not allowed (no global position estimate) or not wanted, try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_DENIED) { @@ -1212,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1226,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) status_changed = true; } @@ -1241,7 +1328,32 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + + // TODO remove code duplication + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; + } } + was_armed = armed.armed; if (main_state_changed) { status_changed = true; @@ -1427,76 +1539,29 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + case SWITCH_POS_MIDDLE: // ASSISTED + if (sp_man->assisted_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT print_reject_mode(status, "EASY"); @@ -1504,29 +1569,33 @@ set_main_state_rc(struct vehicle_status_s *status) res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (sp_man->assisted_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); @@ -1630,8 +1699,11 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; } diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp new file mode 100644 index 000000000..6eceb21f8 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -0,0 +1,2539 @@ +#include "estimator.h" +#include <string.h> + +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include <stdio.h> + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +static void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +static void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +void Vector3f::zero(void) +{ + x = 0.0f; + y = 0.0f; + z = 0.0f; +} + +Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} + +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + useRangeFinder(true), + numericalProtection(true), + refSet(false), + storeIndex(0), + gpsHgt(0.0f), + baroHgt(0.0f), + GPSstatus(0), + VtasMeas(0.0f) +{ + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + + InitialiseParameters(); + ZeroVariables(); +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() +{ + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + float rotationMag; + float qUpdated[4]; + float quatMag; + float deltaQuat[4]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + +// Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z - states[13]; + +// Save current measurements + Vector3f prevDelAng = correctedDelAng; + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + +// Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cosf(0.5f*rotationMag); + float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + +// Normalise the quaternions and update the quaternion states + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + float quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + +// Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) + accNavMag = delVelNav.length()/dtIMU; + +// If calculating position save previous velocity + float lastVelocity[3]; + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + +// Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + +// If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + + // Constrain states (to protect against filter divergence) + ConstrainStates(); +} + +void AttPosEKF::CovariancePrediction(float dt) +{ + // scalars + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + float dvz_b; + + // arrays + float processNoise[n_states]; + float SF[15]; + float SG[8]; + float SQ[11]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; + + // calculate covariance prediction process noise + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + // scale gyro bias noise when on ground to allow for faster bias estimation + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); + if (onGround) dazCov = dazCov * sq(yawVarScale); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); + + // Predicted covariance calculation + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; + nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; + nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; + nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; + nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; + nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); + nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; + nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; + nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; + nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; + nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; + nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; + nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; + nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); + nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; + nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; + nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; + nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; + nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; + nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; + nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; + nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[7][21] = P[7][21] + P[4][21]*dt; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[8][21] = P[8][21] + P[5][21]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[9][21] = P[9][21] + P[6][21]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; + nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[10][21] = P[10][21]; + nextP[10][22] = P[10][22]; + nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; + nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[11][21] = P[11][21]; + nextP[11][22] = P[11][22]; + nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; + nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[12][21] = P[12][21]; + nextP[12][22] = P[12][22]; + nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; + nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[13][21] = P[13][21]; + nextP[13][22] = P[13][22]; + nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; + nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[14][21] = P[14][21]; + nextP[14][22] = P[14][22]; + nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; + nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[15][21] = P[15][21]; + nextP[15][22] = P[15][22]; + nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; + nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[16][21] = P[16][21]; + nextP[16][22] = P[16][22]; + nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; + nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[17][21] = P[17][21]; + nextP[17][22] = P[17][22]; + nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; + nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[18][21] = P[18][21]; + nextP[18][22] = P[18][22]; + nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; + nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[19][21] = P[19][21]; + nextP[19][22] = P[19][22]; + nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; + nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + nextP[20][21] = P[20][21]; + nextP[20][22] = P[20][22]; + nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; + nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; + nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + nextP[21][19] = P[21][19]; + nextP[21][20] = P[21][20]; + nextP[21][21] = P[21][21]; + nextP[21][22] = P[21][22]; + nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; + nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; + nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; + nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; + nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; + nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; + nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; + nextP[22][7] = P[22][7] + P[22][4]*dt; + nextP[22][8] = P[22][8] + P[22][5]*dt; + nextP[22][9] = P[22][9] + P[22][6]*dt; + nextP[22][10] = P[22][10]; + nextP[22][11] = P[22][11]; + nextP[22][12] = P[22][12]; + nextP[22][13] = P[22][13]; + nextP[22][14] = P[22][14]; + nextP[22][15] = P[22][15]; + nextP[22][16] = P[22][16]; + nextP[22][17] = P[22][17]; + nextP[22][18] = P[22][18]; + nextP[22][19] = P[22][19]; + nextP[22][20] = P[22][20]; + nextP[22][21] = P[22][21]; + nextP[22][22] = P[22][22]; + + for (unsigned i = 0; i < n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,16,21); + zeroCols(nextP,16,21); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (unsigned j = 0; j < n_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i <= 13; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } + } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + } + + ConstrainVariances(); +} + +void AttPosEKF::FuseVelposNED() +{ + +// declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + +// declare variables used to check measurement errors + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; + +// declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = sq(vneSigma) + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) + { + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); + } + else + { + current_ekf_state.velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) + { + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); + } + else + { + current_ekf_state.posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + { + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); + } + else + { + current_ekf_state.hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && current_ekf_state.hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 22; + } + else + { + indexLimit = 13; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex >= 0 && obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + +} + +void AttPosEKF::FuseMagnetometer() +{ + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + uint8_t indexLimit; + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 22; + } + else + { + indexLimit = 13; + } + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 1.0f; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseMagData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2*q1*q2 - 2*q0*q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2*q0*q1 + 2*q2*q3; + H_MAG[20] = 1; + + // Calculate Kalman gain + SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2*q0*q2 + 2*q1*q3; + H_MAG[17] = 2*q2*q3 - 2*q0*q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[21] = 1; + + // Calculate Kalman gain + SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j <= 3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 16; j <= 21; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 16; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 16; k<=21; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i <= indexLimit; i++) + { + for (uint8_t j = 0; j <= indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + float R_TAS = sq(airspeedMeasurementSigma); + float SH_TAS[3]; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[14]; + vwe = statesAtVtasMeasTime[15]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; + + float H_TAS[n_states]; + for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[14] = -SH_TAS[2]; + H_TAS[15] = -SH_TAS[1]; + + // Calculate Kalman gains + float SK_TAS = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j <= 22; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j <= 3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in H to reduce the + // number of operations + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 14; k <= 15; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col<n_states; col++) + { + covMat[row][col] = 0.0; + } + } +} + +void AttPosEKF::FuseRangeFinder() +{ + + // Local variables + float rngPred; + float SH_RNG[5]; + float H_RNG[23]; + float SK_RNG[6]; + float cosRngTilt; + const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance + + // Copy required states to local variable names + float q0 = statesAtRngTime[0]; + float q1 = statesAtRngTime[1]; + float q2 = statesAtRngTime[2]; + float q3 = statesAtRngTime[3]; + float pd = statesAtRngTime[9]; + float ptd = statesAtRngTime[22]; + + // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data + SH_RNG[4] = sin(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + if (useRangeFinder && cosRngTilt > 0.87f) + { + // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset + // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations + SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_RNG[1] = pd - ptd; + SH_RNG[2] = 1/sq(SH_RNG[0]); + SH_RNG[3] = 1/SH_RNG[0]; + for (uint8_t i = 0; i < n_states; i++) { + H_RNG[i] = 0.0f; + Kfusion[i] = 0.0f; + } + H_RNG[22] = -SH_RNG[3]; + SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); + SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; + SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; + SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; + SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; + SK_RNG[5] = SH_RNG[2]; + Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + + // Calculate the measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // correct the state vector + states[22] = states[22] - Kfusion[22] * innovRng; + + // correct the covariance P = (I - K*H)*P + P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + } + } + +} + +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (col=first; col<=last; col++) + { + for (row=0; row < n_states; row++) + { + covMat[row][col] = 0.0; + } + } +} + +float AttPosEKF::sq(float valIn) +{ + return valIn*valIn; +} + +// Store states in a history array along with time stamp +void AttPosEKF::StoreStates(uint64_t timestamp_ms) +{ + for (unsigned i=0; i<n_states; i++) + storedStates[i][storeIndex] = states[i]; + statetimeStamp[storeIndex] = timestamp_ms; + storeIndex++; + if (storeIndex == data_buffer_size) + storeIndex = 0; +} + +void AttPosEKF::ResetStoredStates() +{ + // reset all stored states + memset(&storedStates[0][0], 0, sizeof(storedStates)); + memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + + // reset store index to first + storeIndex = 0; + + // overwrite all existing states + for (unsigned i = 0; i < n_states; i++) { + storedStates[i][storeIndex] = states[i]; + } + + statetimeStamp[storeIndex] = millis(); + + // increment to next storage index + storeIndex++; +} + +// Output the state vector stored at the time that best matches that specified by msec +int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) +{ + int ret = 0; + + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t timeDelta; + + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + } + else // otherwise output current state + { + for (unsigned i = 0; i < n_states; i++) { + if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + ret++; + } + } + } + + return ret; +} + +void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) +{ + // Calculate the nav to body cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); +} + +void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +{ + // Calculate the body to nav cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); +} + +void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) +{ + float u1 = cos(0.5f*eul[0]); + float u2 = cos(0.5f*eul[1]); + float u3 = cos(0.5f*eul[2]); + float u4 = sin(0.5f*eul[0]); + float u5 = sin(0.5f*eul[1]); + float u6 = sin(0.5f*eul[2]); + quat[0] = u1*u2*u3+u4*u5*u6; + quat[1] = u4*u2*u3-u1*u5*u6; + quat[2] = u1*u5*u3+u4*u2*u6; + quat[3] = u1*u2*u6-u4*u5*u3; +} + +void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) +{ + y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); + y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); + y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); +} + +void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNED[0] = gpsGndSpd*cosf(gpsCourse); + velNED[1] = gpsGndSpd*sinf(gpsCourse); + velNED[2] = gpsVelD; +} + +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +{ + posNED[0] = earthRadius * (lat - latRef); + posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); + posNED[2] = -(hgt - hgtRef); +} + +void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + lat = latRef + posNED[0] * earthRadiusInv; + lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNED[2]; +} + +void AttPosEKF::OnGroundCheck() +{ + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + if (staticMode) { + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); + } +} + +void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) +{ + //Define Earth rotation vector in the NED navigation frame + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); +} + +void AttPosEKF::CovarianceInit() +{ + // Calculate the initial covariance matrix P + P[0][0] = 0.25f * sq(1.0f*deg2rad); + P[1][1] = 0.25f * sq(1.0f*deg2rad); + P[2][2] = 0.25f * sq(1.0f*deg2rad); + P[3][3] = 0.25f * sq(10.0f*deg2rad); + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} + +void AttPosEKF::ConstrainVariances() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m + + // Constrain quaternion variances + for (unsigned i = 0; i <= 3; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i <= 9; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); + } + + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + + // Wind velocity variances + for (unsigned i = 14; i <= 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 16; i <= 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 19; i <= 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain terrain offset variance + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); +} + +void AttPosEKF::ConstrainStates() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m + + // Constrain quaternion + for (unsigned i = 0; i <= 3; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i <= 6; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i <= 8; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i <= 12; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 14; i <= 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 16; i <= 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 19; i <= 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + +} + +void AttPosEKF::ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool AttPosEKF::FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void AttPosEKF::ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void AttPosEKF::ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void AttPosEKF::ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + + +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) +{ + for (unsigned i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); + err = true; + goto out; + } // delta velocities + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("KH NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("KHP NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("P NaN"); + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); + err = true; + goto out; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, states[i]); + err = true; + goto out; + } // state matrix + } + +out: + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +int AttPosEKF::CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); + + InitializeDynamic(velNED); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return 2; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + return 3; + } + + return 0; +} + +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +{ + + // Clear the init flag + statesInitialised = false; + + ZeroVariables(); + + // Calculate initial filter quaternion states from raw measurements + float initQuat[4]; + Vector3f initMagXYZ; + initMagXYZ = magData - magBias; + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); + + // Calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + Mat3f DCM; + quat2Tbn(DCM, initQuat); + Vector3f initMagNED; + initMagXYZ = magData - magBias; + initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; + initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; + initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = magData.x; + magstate.magE = magData.y; + magstate.magD = magData.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM = DCM; + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities + for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + states[19] = magBias.x; // Magnetic Field Bias X + states[20] = magBias.y; // Magnetic Field Bias Y + states[21] = magBias.z; // Magnetic Field Bias Z + states[22] = 0.0f; // terrain height + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + + //Initialise summed variables used by covariance prediction + summedDelAng.x = 0.0f; + summedDelAng.y = 0.0f; + summedDelAng.z = 0.0f; + summedDelVel.x = 0.0f; + summedDelVel.y = 0.0f; + summedDelVel.z = 0.0f; +} + +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +{ + //store initial lat,long and height + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; + refSet = true; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED); +} + +void AttPosEKF::ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + +} + +void AttPosEKF::GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(*state)); +} + +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); +} diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e62f1a98a..e118ae573 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -20,7 +20,7 @@ public: float z; float length(void) const; - Vector3f zero(void) const; + void zero(void); }; class Mat3f @@ -33,6 +33,7 @@ public: Mat3f(); + void identity(); Mat3f transpose(void) const; }; @@ -45,14 +46,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - -// extern bool staticMode; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -82,6 +78,88 @@ public: AttPosEKF(); ~AttPosEKF(); + + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; + + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -96,6 +174,7 @@ public: float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float statesAtRngTime[n_states]; // filter states at the effective measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -104,6 +183,10 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; @@ -115,26 +198,28 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float rngMea; // Ground distance float posNED[3]; // North, East Down position (m) float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes float innovVtas; // innovation output + float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) + double latRef; // WGS-84 latitude of reference point (rad) + double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS; // ratio f true to equivalent airspeed + unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables float gpsCourse; float gpsVelD; - float gpsLat; - float gpsLon; + double gpsLat; + double gpsLon; float gpsHgt; uint8_t GPSstatus; @@ -148,11 +233,13 @@ public: bool fuseHgtData; // this boolean causes the hgtMea obs to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused + bool fuseRngData; ///< true when range data is fused bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used + bool useRangeFinder; ///< true when rangefinder is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -172,6 +259,10 @@ void FuseMagnetometer(); void FuseAirspeed(); +void FuseRangeFinder(); + +void FuseOpticalFlow(); + void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -192,7 +283,7 @@ void StoreStates(uint64_t timestamp_ms); * time-wise where valid states were updated and invalid remained at the old * value. */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); +int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); @@ -206,7 +297,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); @@ -218,7 +309,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3]); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); float ConstrainFloat(float val, float min, float max); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 46bd399f7..ad8c1b4c0 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/estimator_status.h> #include <uORB/topics/actuator_armed.h> +#include <uORB/topics/home_position.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <geo/geo.h> @@ -90,7 +91,7 @@ * * @ingroup apps */ -extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -177,6 +179,8 @@ private: struct sensor_combined_s _sensor_combined; #endif + struct map_projection_reference_s _pos_ref; + float _baro_ref; /**< barometer reference altitude */ float _baro_gps_offset; /**< offset between GPS and baro */ @@ -190,6 +194,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -199,6 +204,18 @@ private: int32_t height_delay_ms; int32_t mag_delay_ms; int32_t tas_delay_ms; + float velne_noise; + float veld_noise; + float posne_noise; + float posd_noise; + float mag_noise; + float gyro_pnoise; + float acc_pnoise; + float gbias_pnoise; + float abias_pnoise; + float mage_pnoise; + float magb_pnoise; + float eas_noise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -207,6 +224,18 @@ private: param_t height_delay_ms; param_t mag_delay_ms; param_t tas_delay_ms; + param_t velne_noise; + param_t veld_noise; + param_t posne_noise; + param_t posd_noise; + param_t mag_noise; + param_t gyro_pnoise; + param_t acc_pnoise; + param_t gbias_pnoise; + param_t abias_pnoise; + param_t mage_pnoise; + param_t magb_pnoise; + param_t eas_noise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -295,13 +324,25 @@ FixedwingEstimator::FixedwingEstimator() : _ekf(nullptr) { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); + _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE"); + _parameter_handles.veld_noise = param_find("PE_VELD_NOISE"); + _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE"); + _parameter_handles.posd_noise = param_find("PE_POSD_NOISE"); + _parameter_handles.mag_noise = param_find("PE_MAG_NOISE"); + _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE"); + _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE"); + _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE"); + _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); + _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); + _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); + _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); /* fetch initial parameter values */ parameters_update(); @@ -366,6 +407,36 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); + param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise)); + param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise)); + param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise)); + param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise)); + param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise)); + param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise)); + param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise)); + param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise)); + param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); + param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); + param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); + param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); + + if (_ekf) { + // _ekf->yawVarScale = 1.0f; + // _ekf->windVelSigma = 0.1f; + _ekf->dAngBiasSigma = _parameters.gbias_pnoise; + _ekf->dVelBiasSigma = _parameters.abias_pnoise; + _ekf->magEarthSigma = _parameters.mage_pnoise; + _ekf->magBodySigma = _parameters.magb_pnoise; + // _ekf->gndHgtSigma = 0.02f; + _ekf->vneSigma = _parameters.velne_noise; + _ekf->vdSigma = _parameters.veld_noise; + _ekf->posNeSigma = _parameters.posne_noise; + _ekf->posDSigma = _parameters.posd_noise; + _ekf->magMeasurementSigma = _parameters.mag_noise; + _ekf->gyroProcessNoise = _parameters.gyro_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + } return OK; } @@ -395,6 +466,7 @@ float dt = 0.0f; // time lapsed since last covariance prediction void FixedwingEstimator::task_main() { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); @@ -410,6 +482,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -429,6 +502,7 @@ FixedwingEstimator::task_main() orb_set_interval(_sensor_combined_sub, 4); #endif + /* sets also parameters in the EKF object */ parameters_update(); /* set initial filter state */ @@ -533,17 +607,22 @@ FixedwingEstimator::task_main() last_run = _gyro.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + } _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; @@ -573,19 +652,25 @@ FixedwingEstimator::task_main() IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; - last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f || deltaT < 0.000001f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + } + + last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2])) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; @@ -638,6 +723,9 @@ FixedwingEstimator::task_main() } else { + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); @@ -658,6 +746,21 @@ FixedwingEstimator::task_main() _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + newDataGps = true; } @@ -725,6 +828,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -733,26 +838,38 @@ FixedwingEstimator::task_main() { const char* str = "NaN in states, resetting"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; + const char* str = "switching to dynamic state"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } + } + + // XXX trap for testing + if (check == 1) { + errx(1, "NUMERIC ERROR IN FILTER"); } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -784,6 +901,7 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + } @@ -795,22 +913,28 @@ FixedwingEstimator::task_main() // XXX we rather want to check all updated - if (hrt_elapsed_time(&start_time) > 100000) { + if (hrt_elapsed_time(&_gps_start_time) > 50000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + // bool home_set; + // orb_check(_home_sub, &home_set); + // struct home_position_s home; + // orb_copy(ORB_ID(home_position), _home_sub, &home); + + if (!_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lon = _gps.alt; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; @@ -820,9 +944,10 @@ FixedwingEstimator::task_main() _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - // XXX this is not multithreading safe - map_projection_init(lat, lon); - mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); + warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); _gps_initialized = true; @@ -836,7 +961,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); } } @@ -870,10 +995,10 @@ FixedwingEstimator::task_main() // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); - _ekf->summedDelAng = _ekf->summedDelAng.zero(); - _ekf->summedDelVel = _ekf->summedDelVel.zero(); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); dt = 0.0f; } @@ -1042,18 +1167,14 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; - _global_pos.baro_valid = true; - _global_pos.global_valid = true; - if (_local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; } - /* set valid values even if position is not valid */ if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; @@ -1065,16 +1186,15 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - if (_local_pos.z_valid) { - _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; - } - if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */ @@ -1105,7 +1225,7 @@ FixedwingEstimator::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 6000, @@ -1211,10 +1331,10 @@ int FixedwingEstimator::trip_nan() { return ret; } -int fw_att_pos_estimator_main(int argc, char *argv[]) +int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); if (!strcmp(argv[1], "start")) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c new file mode 100644 index 000000000..cfcd99858 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_att_pos_estimator_params.c + * + * Parameters defined by the attitude and position estimator task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Estimator parameters, accessible via MAVLink + * + */ + +/** + * Velocity estimate delay + * + * The delay in milliseconds of the velocity estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230); + +/** + * Position estimate delay + * + * The delay in milliseconds of the position estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210); + +/** + * Height estimate delay + * + * The delay in milliseconds of the height estimate from the barometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350); + +/** + * Mag estimate delay + * + * The delay in milliseconds of the magnetic field estimate from + * the magnetometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30); + +/** + * True airspeeed estimate delay + * + * The delay in milliseconds of the airspeed estimate. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); + +/** + * GPS vs. barometric altitude update weight + * + * RE-CHECK this. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); + +/** + * Airspeed measurement noise. + * + * Increasing this value will make the filter trust this sensor + * less and trust other sensors more. + * + * @min 0.5 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f); + +/** + * Velocity measurement noise in north-east (horizontal) direction. + * + * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); + +/** + * Velocity noise in down (vertical) direction + * + * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); + +/** + * Position noise in north-east (horizontal) direction + * + * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); + +/** + * Position noise in down (vertical) direction + * + * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); + +/** + * Magnetometer measurement noise + * + * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); + +/** + * Gyro process noise + * + * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. + * This noise controls how much the filter trusts the gyro measurements. + * Increasing it makes the filter trust the gyro less and other sensors more. + * + * @min 0.001 + * @max 0.05 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); + +/** + * Accelerometer process noise + * + * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. + * Increasing this value makes the filter trust the accelerometer less + * and other sensors more. + * + * @min 0.05 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); + +/** + * Gyro bias estimate process noise + * + * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. + * Increasing this value will make the gyro bias converge faster but noisier. + * + * @min 0.0000001 + * @max 0.00001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); + +/** + * Accelerometer bias estimate process noise + * + * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Increasing this value makes the bias estimation faster and noisier. + * + * @min 0.0001 + * @max 0.001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); + +/** + * Magnetometer earth frame offsets process noise + * + * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. + * Increasing this value makes the magnetometer earth bias estimate converge + * faster but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); + +/** + * Magnetometer body frame offsets process noise + * + * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. + * Increasing this value makes the magnetometer body bias estimate converge faster + * but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); + diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index c992959e0..30955d0dd 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -35,7 +35,7 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = fw_att_pos_estimator +MODULE_COMMAND = ekf_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2f84dc963..81bef0db3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,6 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -211,6 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; + param_t man_roll_max; + param_t man_pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); + _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + /* fetch initial parameter values */ parameters_update(); } @@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); + param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); + _parameters.man_roll_max = math::radians(_parameters.man_roll_max); + _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ @@ -706,8 +717,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -771,7 +782,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -780,16 +791,16 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } /* @@ -815,7 +826,7 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; + _actuators.control[1] = -_manual.pitch; _actuators.control[2] = _manual.yaw; _actuators.control[3] = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c80a44f2a..aa637e207 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +// @DisplayName Max Manual Roll +// @Description Max roll for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); + +// @DisplayName Max Manual Pitch +// @Description Max pitch for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp deleted file mode 100644 index c31217393..000000000 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ /dev/null @@ -1,2248 +0,0 @@ -#include "estimator.h" - -#include <string.h> - -float Vector3f::length(void) const -{ - return sqrt(x*x + y*y + z*z); -} - -Vector3f Vector3f::zero(void) const -{ - Vector3f ret = *this; - ret.x = 0.0; - ret.y = 0.0; - ret.z = 0.0; - return ret; -} - -Mat3f::Mat3f() { - x.x = 1.0f; - x.y = 0.0f; - x.z = 0.0f; - - y.x = 0.0f; - y.y = 1.0f; - y.z = 0.0f; - - z.x = 0.0f; - z.y = 0.0f; - z.z = 1.0f; -} - -Mat3f Mat3f::transpose(void) const -{ - Mat3f ret = *this; - swap_var(ret.x.y, ret.y.x); - swap_var(ret.x.z, ret.z.x); - swap_var(ret.y.z, ret.z.y); - return ret; -} - -// overload + operator to provide a vector addition -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x + vecIn2.x; - vecOut.y = vecIn1.y + vecIn2.y; - vecOut.z = vecIn1.z + vecIn2.z; - return vecOut; -} - -// overload - operator to provide a vector subtraction -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x - vecIn2.x; - vecOut.y = vecIn1.y - vecIn2.y; - vecOut.z = vecIn1.z - vecIn2.z; - return vecOut; -} - -// overload * operator to provide a matrix vector product -Vector3f operator*( Mat3f matIn, Vector3f vecIn) -{ - Vector3f vecOut; - vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; - vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; - return vecOut; -} - -// overload % operator to provide a vector cross product -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; - vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; - vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(Vector3f vecIn1, float sclIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(float sclIn1, Vector3f vecIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -void swap_var(float &d1, float &d2) -{ - float tmp = d1; - d1 = d2; - d2 = tmp; -} - -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - EAS2TAS(1.0f), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - numericalProtection(true), - storeIndex(0) -{ - -} - -AttPosEKF::~AttPosEKF() -{ -} - -void AttPosEKF::UpdateStrapdownEquationsNED() -{ - Vector3f delVelNav; - float q00; - float q11; - float q22; - float q33; - float q01; - float q02; - float q03; - float q12; - float q13; - float q23; - Mat3f Tbn; - Mat3f Tnb; - float rotationMag; - float qUpdated[4]; - float quatMag; - float deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - -// Remove sensor bias errors - correctedDelAng.x = dAngIMU.x - states[10]; - correctedDelAng.y = dAngIMU.y - states[11]; - correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; - -// Save current measurements - Vector3f prevDelAng = correctedDelAng; - -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - -// Convert the rotation vector to its equivalent quaternion - rotationMag = correctedDelAng.length(); - if (rotationMag < 1e-12f) - { - deltaQuat[0] = 1.0; - deltaQuat[1] = 0.0; - deltaQuat[2] = 0.0; - deltaQuat[3] = 0.0; - } - else - { - deltaQuat[0] = cos(0.5f*rotationMag); - float rotScaler = (sin(0.5f*rotationMag))/rotationMag; - deltaQuat[1] = correctedDelAng.x*rotScaler; - deltaQuat[2] = correctedDelAng.y*rotScaler; - deltaQuat[3] = correctedDelAng.z*rotScaler; - } - -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion - qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; - qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; - qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; - qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - -// Normalise the quaternions and update the quaternion states - quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); - if (quatMag > 1e-16f) - { - float quatMagInv = 1.0f/quatMag; - states[0] = quatMagInv*qUpdated[0]; - states[1] = quatMagInv*qUpdated[1]; - states[2] = quatMagInv*qUpdated[2]; - states[3] = quatMagInv*qUpdated[3]; - } - -// Calculate the body to nav cosine matrix - q00 = sq(states[0]); - q11 = sq(states[1]); - q22 = sq(states[2]); - q33 = sq(states[3]); - q01 = states[0]*states[1]; - q02 = states[0]*states[2]; - q03 = states[0]*states[3]; - q12 = states[1]*states[2]; - q13 = states[1]*states[3]; - q23 = states[2]*states[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); - - Tnb = Tbn.transpose(); - -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded - //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) - accNavMag = delVelNav.length()/dtIMU; - -// If calculating position save previous velocity - float lastVelocity[3]; - lastVelocity[0] = states[4]; - lastVelocity[1] = states[5]; - lastVelocity[2] = states[6]; - -// Sum delta velocities to get velocity - states[4] = states[4] + delVelNav.x; - states[5] = states[5] + delVelNav.y; - states[6] = states[6] + delVelNav.z; - -// If calculating postions, do a trapezoidal integration for position - states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; - states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; - states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - - // Constrain states (to protect against filter divergence) - ConstrainStates(); -} - -void AttPosEKF::CovariancePrediction(float dt) -{ - // scalars - float windVelSigma; - float dAngBiasSigma; - // float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - - // arrays - float processNoise[21]; - float SF[14]; - float SG[8]; - float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; - - // calculate covariance prediction process noise - const float yawVarScale = 1.0f; - windVelSigma = dt*0.1f; - dAngBiasSigma = dt*5.0e-7f; - magEarthSigma = dt*3.0e-4f; - magBodySigma = dt*3.0e-4f; - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; - for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; - for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; - for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; - for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - - // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); - if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); - - // Predicted covariance calculation - SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; - SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SF[3] = day/2 - day_b/2; - SF[4] = daz/2 - daz_b/2; - SF[5] = dax/2 - dax_b/2; - SF[6] = dax_b/2 - dax/2; - SF[7] = daz_b/2 - daz/2; - SF[8] = day_b/2 - day/2; - SF[9] = q1/2; - SF[10] = q2/2; - SF[11] = q3/2; - SF[12] = 2*dvz*q0; - SF[13] = 2*dvy*q1; - - SG[0] = q0/2; - SG[1] = sq(q3); - SG[2] = sq(q2); - SG[3] = sq(q1); - SG[4] = sq(q0); - SG[5] = 2*q2*q3; - SG[6] = 2*q1*q3; - SG[7] = 2*q1*q2; - - SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); - SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); - SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); - SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; - SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; - SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; - SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; - SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; - SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; - SQ[9] = sq(SG[0]); - SQ[10] = sq(q1); - - SPP[0] = SF[12] + SF[13] - 2*dvx*q2; - SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SPP[3] = SF[11]; - SPP[4] = SF[10]; - SPP[5] = SF[9]; - SPP[6] = SF[7]; - SPP[7] = SF[8]; - - nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); - nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); - nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); - nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); - nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); - nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); - nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; - nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; - nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; - nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; - nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; - nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; - nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; - nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; - nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; - nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; - nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); - nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); - nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); - nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); - nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); - nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; - nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; - nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; - nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; - nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; - nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; - nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; - nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; - nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; - nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; - nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; - nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); - nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); - nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); - nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); - nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); - nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); - nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; - nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; - nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; - nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; - nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; - nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; - nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; - nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; - nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; - nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; - nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; - nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); - nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); - nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); - nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); - nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); - nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; - nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; - nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; - nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; - nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; - nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; - nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; - nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; - nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; - nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; - nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); - nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); - nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); - nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); - nextP[7][10] = P[7][10] + P[4][10]*dt; - nextP[7][11] = P[7][11] + P[4][11]*dt; - nextP[7][12] = P[7][12] + P[4][12]*dt; - nextP[7][13] = P[7][13] + P[4][13]*dt; - nextP[7][14] = P[7][14] + P[4][14]*dt; - nextP[7][15] = P[7][15] + P[4][15]*dt; - nextP[7][16] = P[7][16] + P[4][16]*dt; - nextP[7][17] = P[7][17] + P[4][17]*dt; - nextP[7][18] = P[7][18] + P[4][18]*dt; - nextP[7][19] = P[7][19] + P[4][19]*dt; - nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); - nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); - nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); - nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); - nextP[8][10] = P[8][10] + P[5][10]*dt; - nextP[8][11] = P[8][11] + P[5][11]*dt; - nextP[8][12] = P[8][12] + P[5][12]*dt; - nextP[8][13] = P[8][13] + P[5][13]*dt; - nextP[8][14] = P[8][14] + P[5][14]*dt; - nextP[8][15] = P[8][15] + P[5][15]*dt; - nextP[8][16] = P[8][16] + P[5][16]*dt; - nextP[8][17] = P[8][17] + P[5][17]*dt; - nextP[8][18] = P[8][18] + P[5][18]*dt; - nextP[8][19] = P[8][19] + P[5][19]*dt; - nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); - nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); - nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); - nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); - nextP[9][10] = P[9][10] + P[6][10]*dt; - nextP[9][11] = P[9][11] + P[6][11]*dt; - nextP[9][12] = P[9][12] + P[6][12]*dt; - nextP[9][13] = P[9][13] + P[6][13]*dt; - nextP[9][14] = P[9][14] + P[6][14]*dt; - nextP[9][15] = P[9][15] + P[6][15]*dt; - nextP[9][16] = P[9][16] + P[6][16]*dt; - nextP[9][17] = P[9][17] + P[6][17]*dt; - nextP[9][18] = P[9][18] + P[6][18]*dt; - nextP[9][19] = P[9][19] + P[6][19]*dt; - nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; - nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; - nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; - nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; - nextP[10][7] = P[10][7] + P[10][4]*dt; - nextP[10][8] = P[10][8] + P[10][5]*dt; - nextP[10][9] = P[10][9] + P[10][6]*dt; - nextP[10][10] = P[10][10]; - nextP[10][11] = P[10][11]; - nextP[10][12] = P[10][12]; - nextP[10][13] = P[10][13]; - nextP[10][14] = P[10][14]; - nextP[10][15] = P[10][15]; - nextP[10][16] = P[10][16]; - nextP[10][17] = P[10][17]; - nextP[10][18] = P[10][18]; - nextP[10][19] = P[10][19]; - nextP[10][20] = P[10][20]; - nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; - nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; - nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; - nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; - nextP[11][7] = P[11][7] + P[11][4]*dt; - nextP[11][8] = P[11][8] + P[11][5]*dt; - nextP[11][9] = P[11][9] + P[11][6]*dt; - nextP[11][10] = P[11][10]; - nextP[11][11] = P[11][11]; - nextP[11][12] = P[11][12]; - nextP[11][13] = P[11][13]; - nextP[11][14] = P[11][14]; - nextP[11][15] = P[11][15]; - nextP[11][16] = P[11][16]; - nextP[11][17] = P[11][17]; - nextP[11][18] = P[11][18]; - nextP[11][19] = P[11][19]; - nextP[11][20] = P[11][20]; - nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; - nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; - nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; - nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; - nextP[12][7] = P[12][7] + P[12][4]*dt; - nextP[12][8] = P[12][8] + P[12][5]*dt; - nextP[12][9] = P[12][9] + P[12][6]*dt; - nextP[12][10] = P[12][10]; - nextP[12][11] = P[12][11]; - nextP[12][12] = P[12][12]; - nextP[12][13] = P[12][13]; - nextP[12][14] = P[12][14]; - nextP[12][15] = P[12][15]; - nextP[12][16] = P[12][16]; - nextP[12][17] = P[12][17]; - nextP[12][18] = P[12][18]; - nextP[12][19] = P[12][19]; - nextP[12][20] = P[12][20]; - nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; - nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; - nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; - nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; - nextP[13][7] = P[13][7] + P[13][4]*dt; - nextP[13][8] = P[13][8] + P[13][5]*dt; - nextP[13][9] = P[13][9] + P[13][6]*dt; - nextP[13][10] = P[13][10]; - nextP[13][11] = P[13][11]; - nextP[13][12] = P[13][12]; - nextP[13][13] = P[13][13]; - nextP[13][14] = P[13][14]; - nextP[13][15] = P[13][15]; - nextP[13][16] = P[13][16]; - nextP[13][17] = P[13][17]; - nextP[13][18] = P[13][18]; - nextP[13][19] = P[13][19]; - nextP[13][20] = P[13][20]; - nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; - nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; - nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; - nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; - nextP[14][7] = P[14][7] + P[14][4]*dt; - nextP[14][8] = P[14][8] + P[14][5]*dt; - nextP[14][9] = P[14][9] + P[14][6]*dt; - nextP[14][10] = P[14][10]; - nextP[14][11] = P[14][11]; - nextP[14][12] = P[14][12]; - nextP[14][13] = P[14][13]; - nextP[14][14] = P[14][14]; - nextP[14][15] = P[14][15]; - nextP[14][16] = P[14][16]; - nextP[14][17] = P[14][17]; - nextP[14][18] = P[14][18]; - nextP[14][19] = P[14][19]; - nextP[14][20] = P[14][20]; - nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; - nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; - nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; - nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; - nextP[15][7] = P[15][7] + P[15][4]*dt; - nextP[15][8] = P[15][8] + P[15][5]*dt; - nextP[15][9] = P[15][9] + P[15][6]*dt; - nextP[15][10] = P[15][10]; - nextP[15][11] = P[15][11]; - nextP[15][12] = P[15][12]; - nextP[15][13] = P[15][13]; - nextP[15][14] = P[15][14]; - nextP[15][15] = P[15][15]; - nextP[15][16] = P[15][16]; - nextP[15][17] = P[15][17]; - nextP[15][18] = P[15][18]; - nextP[15][19] = P[15][19]; - nextP[15][20] = P[15][20]; - nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; - nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; - nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; - nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; - nextP[16][7] = P[16][7] + P[16][4]*dt; - nextP[16][8] = P[16][8] + P[16][5]*dt; - nextP[16][9] = P[16][9] + P[16][6]*dt; - nextP[16][10] = P[16][10]; - nextP[16][11] = P[16][11]; - nextP[16][12] = P[16][12]; - nextP[16][13] = P[16][13]; - nextP[16][14] = P[16][14]; - nextP[16][15] = P[16][15]; - nextP[16][16] = P[16][16]; - nextP[16][17] = P[16][17]; - nextP[16][18] = P[16][18]; - nextP[16][19] = P[16][19]; - nextP[16][20] = P[16][20]; - nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; - nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; - nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; - nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; - nextP[17][7] = P[17][7] + P[17][4]*dt; - nextP[17][8] = P[17][8] + P[17][5]*dt; - nextP[17][9] = P[17][9] + P[17][6]*dt; - nextP[17][10] = P[17][10]; - nextP[17][11] = P[17][11]; - nextP[17][12] = P[17][12]; - nextP[17][13] = P[17][13]; - nextP[17][14] = P[17][14]; - nextP[17][15] = P[17][15]; - nextP[17][16] = P[17][16]; - nextP[17][17] = P[17][17]; - nextP[17][18] = P[17][18]; - nextP[17][19] = P[17][19]; - nextP[17][20] = P[17][20]; - nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; - nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; - nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; - nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; - nextP[18][7] = P[18][7] + P[18][4]*dt; - nextP[18][8] = P[18][8] + P[18][5]*dt; - nextP[18][9] = P[18][9] + P[18][6]*dt; - nextP[18][10] = P[18][10]; - nextP[18][11] = P[18][11]; - nextP[18][12] = P[18][12]; - nextP[18][13] = P[18][13]; - nextP[18][14] = P[18][14]; - nextP[18][15] = P[18][15]; - nextP[18][16] = P[18][16]; - nextP[18][17] = P[18][17]; - nextP[18][18] = P[18][18]; - nextP[18][19] = P[18][19]; - nextP[18][20] = P[18][20]; - nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; - nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; - nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; - nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; - nextP[19][7] = P[19][7] + P[19][4]*dt; - nextP[19][8] = P[19][8] + P[19][5]*dt; - nextP[19][9] = P[19][9] + P[19][6]*dt; - nextP[19][10] = P[19][10]; - nextP[19][11] = P[19][11]; - nextP[19][12] = P[19][12]; - nextP[19][13] = P[19][13]; - nextP[19][14] = P[19][14]; - nextP[19][15] = P[19][15]; - nextP[19][16] = P[19][16]; - nextP[19][17] = P[19][17]; - nextP[19][18] = P[19][18]; - nextP[19][19] = P[19][19]; - nextP[19][20] = P[19][20]; - nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; - nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; - nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; - nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; - nextP[20][7] = P[20][7] + P[20][4]*dt; - nextP[20][8] = P[20][8] + P[20][5]*dt; - nextP[20][9] = P[20][9] + P[20][6]*dt; - nextP[20][10] = P[20][10]; - nextP[20][11] = P[20][11]; - nextP[20][12] = P[20][12]; - nextP[20][13] = P[20][13]; - nextP[20][14] = P[20][14]; - nextP[20][15] = P[20][15]; - nextP[20][16] = P[20][16]; - nextP[20][17] = P[20][17]; - nextP[20][18] = P[20][18]; - nextP[20][19] = P[20][19]; - nextP[20][20] = P[20][20]; - - for (unsigned i = 0; i < n_states; i++) - { - nextP[i][i] = nextP[i][i] + processNoise[i]; - } - - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); - } - - // If the total position variance exceds 1E6 (1000m), then stop covariance - // growth by setting the predicted to the previous values - // This prevent an ill conditioned matrix from occurring for long periods - // without GPS - if ((P[7][7] + P[8][8]) > 1E6f) - { - for (uint8_t i=7; i<=8; i++) - { - for (unsigned j = 0; j < n_states; j++) - { - nextP[i][j] = P[i][j]; - nextP[j][i] = P[j][i]; - } - } - } - - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i < 14; i++) { - P[i][i] = nextP[i][i]; - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 12) || (j > 12)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } - - } - - ConstrainVariances(); -} - -void AttPosEKF::FuseVelposNED() -{ - -// declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time - uint32_t horizRetryTime; - -// declare variables used to check measurement errors - float velInnov[3] = {0.0f,0.0f,0.0f}; - float posInnov[2] = {0.0f,0.0f}; - float hgtInnov = 0.0f; - -// declare variables used to control access to arrays - bool fuseData[6] = {false,false,false,false,false,false}; - uint8_t stateIndex; - uint8_t obsIndex; - uint8_t indexLimit; - -// declare variables used by state and covariance update calculations - float velErr; - float posErr; - float R_OBS[6]; - float observation[6]; - float SK; - float quatMag; - -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (fuseVelData || fusePosData || fuseHgtData) - { - // set the GPS data timeout depending on whether airspeed data is present - if (useAirspeed) horizRetryTime = gpsRetryTime; - else horizRetryTime = gpsRetryTimeNoTAS; - - // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; - observation[5] = -(hgtMea); - - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); - R_OBS[1] = R_OBS[0]; - R_OBS[2] = 0.08f + sq(velErr); - R_OBS[3] = R_OBS[2]; - R_OBS[4] = 4.0f + sq(posErr); - R_OBS[5] = 4.0f; - - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } - // calculate innovations and check GPS data validity using an innovation consistency check - if (fuseVelData) - { - // test velocity measurements - uint8_t imax = 2; - if (fusionModeGPS == 1) imax = 1; - for (uint8_t i = 0; i<=imax; i++) - { - velInnov[i] = statesAtVelTime[i+4] - velNED[i]; - stateIndex = 4 + i; - varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - } - // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { - current_ekf_state.velHealth = true; - current_ekf_state.velFailTime = millis(); - } - else - { - current_ekf_state.velHealth = false; - } - } - if (fusePosData) - { - // test horizontal position measurements - posInnov[0] = statesAtPosTime[7] - posNE[0]; - posInnov[1] = statesAtPosTime[8] - posNE[1]; - varInnovVelPos[3] = P[7][7] + R_OBS[3]; - varInnovVelPos[4] = P[8][8] + R_OBS[4]; - // apply a 10-sigma threshold - current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; - if (current_ekf_state.posHealth || current_ekf_state.posTimeout) - { - current_ekf_state.posHealth = true; - current_ekf_state.posFailTime = millis(); - } - else - { - current_ekf_state.posHealth = false; - } - } - // test height measurements - if (fuseHgtData) - { - hgtInnov = statesAtHgtTime[9] + hgtMea; - varInnovVelPos[5] = P[9][9] + R_OBS[5]; - // apply a 10-sigma threshold - current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) - { - current_ekf_state.hgtHealth = true; - current_ekf_state.hgtFailTime = millis(); - } - else - { - current_ekf_state.hgtHealth = false; - } - } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - fuseData[2] = true; - } - if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - } - if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) - { - fuseData[3] = true; - fuseData[4] = true; - } - if (fuseHgtData && current_ekf_state.hgtHealth) - { - fuseData[5] = true; - } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - // Fuse measurements sequentially - for (obsIndex=0; obsIndex<=5; obsIndex++) - { - if (fuseData[obsIndex]) - { - stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) - { - innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 3 || obsIndex == 4) - { - innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 5) - { - innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; - } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging - varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; - for (uint8_t i= 0; i<=indexLimit; i++) - { - Kfusion[i] = P[i][stateIndex]*SK; - } - // Calculate state corrections and re-normalise the quaternions - for (uint8_t i = 0; i<=indexLimit; i++) - { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) // divide by 0 protection - { - for (uint8_t i = 0; i<=3; i++) - { - states[i] = states[i] / quatMag; - } - } - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - KHP[i][j] = Kfusion[i] * P[stateIndex][j]; - } - } - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); - - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); -} - -void AttPosEKF::FuseMagnetometer() -{ - uint8_t obsIndex; - uint8_t indexLimit; - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; - float MagPred[3] = {0.0f,0.0f,0.0f}; - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) - { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - - static float R_MAG = 0.0025f; - - float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseMagData) - { - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - - // Copy required states to local variable names - q0 = statesAtMagMeasTime[0]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM[0][1] = 2*(q1*q2 + q0*q3); - DCM[0][2] = 2*(q1*q3-q0*q2); - DCM[1][0] = 2*(q1*q2 - q0*q3); - DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM[1][2] = 2*(q2*q3 + q0*q1); - DCM[2][0] = 2*(q1*q3 + q0*q2); - DCM[2][1] = 2*(q2*q3 - q0*q1); - DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; - MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; - MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - - // scale magnetometer observation error with total angular rate - R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; - SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2*magN*q0; - SH_MAG[8] = 2*magE*q3; - - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[1] = SH_MAG[0]; - H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; - H_MAG[3] = SH_MAG[2]; - H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[16] = 2*q0*q3 + 2*q1*q2; - H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1.0f; - - // Calculate Kalman gain - SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MX[4] = 2*q0*q2 - 2*q1*q3; - SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); - Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); - varInnovMag[0] = 1.0f/SK_MX[0]; - innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[2]; - H_MAG[1] = SH_MAG[1]; - H_MAG[2] = SH_MAG[0]; - H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; - H_MAG[15] = 2*q1*q2 - 2*q0*q3; - H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; - H_MAG[17] = 2*q0*q1 + 2*q2*q3; - H_MAG[19] = 1; - - // Calculate Kalman gain - SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; - SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MY[3] = 2*q0*q3 - 2*q1*q2; - SK_MY[4] = 2*q0*q1 + 2*q2*q3; - Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); - Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); - Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); - Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); - Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); - Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); - Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); - Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); - Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); - Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); - Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); - Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); - Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); - Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); - varInnovMag[1] = 1.0f/SK_MY[0]; - innovMag[1] = MagPred[1] - magData.y; - } - else if (obsIndex == 2) // we are now fusing the Z measurement - { - // Calculate observation jacobians - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[1]; - H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; - H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[3] = SH_MAG[0]; - H_MAG[15] = 2*q0*q2 + 2*q1*q3; - H_MAG[16] = 2*q2*q3 - 2*q0*q1; - H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - H_MAG[20] = 1; - - // Calculate Kalman gain - SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MZ[4] = 2*q0*q1 - 2*q2*q3; - SK_MZ[5] = 2*q0*q2 + 2*q1*q3; - Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); - Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); - Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); - Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); - Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); - Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); - Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); - Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); - Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); - Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); - Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); - Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); - Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); - Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); - varInnovMag[2] = 1.0f/SK_MZ[0]; - innovMag[2] = MagPred[2] - magData.z; - - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) - { - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=3; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; - if (!onGround) - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - } - else - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = 0.0f; - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k<=3; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - if (!onGround) - { - for (uint8_t k = 15; k<=20; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::FuseAirspeed() -{ - float vn; - float ve; - float vd; - float vwn; - float vwe; - const float R_TAS = 2.0f; - float SH_TAS[3]; - float Kfusion[21]; - float VtasPred; - - // Copy required states to local variable names - vn = statesAtVtasMeasTime[4]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[13]; - vwe = statesAtVtasMeasTime[14]; - - // Need to check that it is flying before fusing airspeed data - // Calculate the predicted airspeed - VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); - // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) - { - // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - - float H_TAS[21]; - for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; - H_TAS[4] = SH_TAS[2]; - H_TAS[5] = SH_TAS[1]; - H_TAS[6] = vd*SH_TAS[0]; - H_TAS[13] = -SH_TAS[2]; - H_TAS[14] = -SH_TAS[1]; - - // Calculate Kalman gains - float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - varInnovVtas = 1.0f/SK_TAS; - - // Calculate the measurement innovation - innovVtas = VtasPred - VtasMeas; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) - { - // correct the state vector - for (uint8_t j=0; j<=20; j++) - { - states[j] = states[j] - Kfusion[j] * innovVtas; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; - for (uint8_t j = 13; j<=14; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 13; k<=14; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col<n_states; col++) - { - covMat[row][col] = 0.0; - } - } -} - -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (col=first; col<=last; col++) - { - for (row=0; row < n_states; row++) - { - covMat[row][col] = 0.0; - } - } -} - -float AttPosEKF::sq(float valIn) -{ - return valIn*valIn; -} - -// Store states in a history array along with time stamp -void AttPosEKF::StoreStates(uint64_t timestamp_ms) -{ - for (unsigned i=0; i<n_states; i++) - storedStates[i][storeIndex] = states[i]; - statetimeStamp[storeIndex] = timestamp_ms; - storeIndex++; - if (storeIndex == data_buffer_size) - storeIndex = 0; -} - -void AttPosEKF::ResetStoredStates() -{ - // reset all stored states - memset(&storedStates[0][0], 0, sizeof(storedStates)); - memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); - - // reset store index to first - storeIndex = 0; - - // overwrite all existing states - for (unsigned i = 0; i < n_states; i++) { - storedStates[i][storeIndex] = states[i]; - } - - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; -} - -// Output the state vector stored at the time that best matches that specified by msec -int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) -{ - int ret = 0; - - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // The time delta can also end up as negative number, - // // since we might compare future to past or past to future - // // therefore cast to int64. - // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - // if (timeDelta < 0) { - // timeDelta = -timeDelta; - // } - - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) { - // if (isfinite(storedStates[i][bestStoreIndex])) { - // statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } else if (isfinite(states[i])) { - // statesForFusion[i] = states[i]; - // } else { - // // There is not much we can do here, except reporting the error we just - // // found. - // ret++; - // } - // } - // else // otherwise output current state - // { - for (uint8_t i=0; i < n_states; i++) { - if (isfinite(states[i])) { - statesForFusion[i] = states[i]; - } else { - ret++; - } - } - // } - - return ret; -} - -void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) -{ - // Calculate the nav to body cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tnb.x.x = q00 + q11 - q22 - q33; - Tnb.y.y = q00 - q11 + q22 - q33; - Tnb.z.z = q00 - q11 - q22 + q33; - Tnb.y.x = 2*(q12 - q03); - Tnb.z.x = 2*(q13 + q02); - Tnb.x.y = 2*(q12 + q03); - Tnb.z.y = 2*(q23 - q01); - Tnb.x.z = 2*(q13 - q02); - Tnb.y.z = 2*(q23 + q01); -} - -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) -{ - // Calculate the body to nav cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); -} - -void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) -{ - float u1 = cos(0.5f*eul[0]); - float u2 = cos(0.5f*eul[1]); - float u3 = cos(0.5f*eul[2]); - float u4 = sin(0.5f*eul[0]); - float u5 = sin(0.5f*eul[1]); - float u6 = sin(0.5f*eul[2]); - quat[0] = u1*u2*u3+u4*u5*u6; - quat[1] = u4*u2*u3-u1*u5*u6; - quat[2] = u1*u5*u3+u4*u2*u6; - quat[3] = u1*u2*u6-u4*u5*u3; -} - -void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) -{ - y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); - y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); - y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); -} - -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) -{ - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); -} - -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) -{ - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - -void AttPosEKF::OnGroundCheck() -{ - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); - if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); - } -} - -void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) -{ - //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cosf(latitude); - omega.y = 0.0f; - omega.z = -earthRate*sinf(latitude); -} - -void AttPosEKF::CovarianceInit() -{ - // Calculate the initial covariance matrix P - P[0][0] = 0.25f * sq(1.0f*deg2rad); - P[1][1] = 0.25f * sq(1.0f*deg2rad); - P[2][2] = 0.25f * sq(1.0f*deg2rad); - P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7); - P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); - P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); - P[11][11] = P[10][10]; - P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; -} - -float AttPosEKF::ConstrainFloat(float val, float min, float max) -{ - return (val > max) ? max : ((val < min) ? min : val); -} - -void AttPosEKF::ConstrainVariances() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Constrain position variances - for (unsigned i = 7; i < 10; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); - } - - // Angle bias variances - for (unsigned i = 10; i < 13; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); - } - - // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Earth magnetic field variances - for (unsigned i = 15; i < 18; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Body magnetic field variances - for (unsigned i = 18; i < 21; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - -} - -void AttPosEKF::ConstrainStates() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - - // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i < 7; i++) { - states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); - } - - // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i < 9; i++) { - states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); - } - - // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); - - // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); - } - - // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { - states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); - } - - // Earth magnetic field limits (in Gauss) - for (unsigned i = 15; i < 18; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Body magnetic field variances (in Gauss). - // the max offset should be in this range. - for (unsigned i = 18; i < 21; i++) { - states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); - } - -} - -void AttPosEKF::ForceSymmetry() -{ - if (!numericalProtection) { - return; - } - - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (P[i][j] + P[j][i]); - P[j][i] = P[i][j]; - } - } -} - -bool AttPosEKF::FilterHealthy() -{ - if (!statesInitialised) { - return false; - } - - // XXX Check state vector for NaNs and ill-conditioning - - // Check if any of the major inputs timed out - if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { - return false; - } - - // Nothing fired, return ok. - return true; -} - -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ -void AttPosEKF::ResetPosition(void) -{ - if (staticMode) { - states[7] = 0; - states[8] = 0; - } else if (GPSstatus >= GPS_FIX_3D) { - - // reset the states from the GPS measurements - states[7] = posNE[0]; - states[8] = posNE[1]; - } -} - -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ -void AttPosEKF::ResetHeight(void) -{ - // write to the state vector - states[9] = -hgtMea; -} - -/** - * Reset the velocity state. - */ -void AttPosEKF::ResetVelocity(void) -{ - if (staticMode) { - states[4] = 0.0f; - states[5] = 0.0f; - states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { - - states[4] = velNED[0]; // north velocity from last reading - states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading - } -} - - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (int i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { - bool err = false; - - // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - if (!isfinite(KH[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // covariance matrix - } - - if (!isfinite(Kfusion[i])) { - - err_report->kalmanGainsNaN = true; - err = true; - } // Kalman gains - - if (!isfinite(states[i])) { - - err_report->statesNaN = true; - err = true; - } // state matrix - } - - if (err) { - FillErrorReport(err_report); - } - - return err; - -} - -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ -int AttPosEKF::CheckAndBound() -{ - - // Store the old filter state - bool currStaticMode = staticMode; - - // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { - - InitializeDynamic(velNED); - - return 1; - } - - // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { - - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - // that's all we can do here, return - return 2; - } - - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - - // Check if we switched between states - if (currStaticMode != staticMode) { - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - return 3; - } - - return 0; -} - -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; -} - -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) -{ - - // Clear the init flag - statesInitialised = false; - - ZeroVariables(); - - // Calculate initial filter quaternion states from raw measurements - float initQuat[4]; - Vector3f initMagXYZ; - initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); - - // Calculate initial Tbn matrix and rotate Mag measurements into NED - // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); - Vector3f initMagNED; - initMagXYZ = magData - magBias; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - - - - // write to state vector - for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z - - statesInitialised = true; - - // initialise the covariance matrix - CovarianceInit(); - - //Define Earth rotation vector in the NED navigation frame - calcEarthRateNED(earthRateNED, latRef); - - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; -} - -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) -{ - //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; - - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - - InitializeDynamic(initvelNED); -} - -void AttPosEKF::ZeroVariables() -{ - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } - - memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); -} - -void AttPosEKF::GetFilterState(struct ekf_status_report *state) -{ - memcpy(state, ¤t_ekf_state, sizeof(state)); -} - -void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) -{ - memcpy(last_error, &last_ekf_error, sizeof(last_error)); -} diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c deleted file mode 100644 index 6138ef39c..000000000 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_att_pos_estimator_params.c - * - * Parameters defined by the attitude and position estimator task - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <nuttx/config.h> - -#include <systemlib/param/param.h> - -/* - * Estimator parameters, accessible via MAVLink - * - */ - -/** - * Velocity estimate delay - * - * The delay in milliseconds of the velocity estimate from GPS. - * - * @min 0 - * @max 1000 - * @group Position Estimator - */ -PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230); - -/** - * Position estimate delay - * - * The delay in milliseconds of the position estimate from GPS. - * - * @min 0 - * @max 1000 - * @group Position Estimator - */ -PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210); - -/** - * Height estimate delay - * - * The delay in milliseconds of the height estimate from the barometer. - * - * @min 0 - * @max 1000 - * @group Position Estimator - */ -PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350); - -/** - * Mag estimate delay - * - * The delay in milliseconds of the magnetic field estimate from - * the magnetometer. - * - * @min 0 - * @max 1000 - * @group Position Estimator - */ -PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30); - -/** - * True airspeeed estimate delay - * - * The delay in milliseconds of the airspeed estimate. - * - * @min 0 - * @max 1000 - * @group Position Estimator - */ -PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); - -/** - * GPS vs. barometric altitude update weight - * - * RE-CHECK this. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator - */ -PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); - diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7ecca0d65..d66d36744 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + warnx("TX FAIL"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fc2998646..33a4fef12 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - _manual_sub(-1), - _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -259,6 +257,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) memset(&f, 0, sizeof(f)); f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -429,47 +428,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - - rc.timestamp = hrt_absolute_time(); - rc.chan_count = 4; + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - rc.chan[0].scaled = man.x / 1000.0f; - rc.chan[1].scaled = man.y / 1000.0f; - rc.chan[2].scaled = man.r / 1000.0f; - rc.chan[3].scaled = man.z / 1000.0f; + manual.timestamp = hrt_absolute_time(); + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; - if (_rc_pub < 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); - - manual.timestamp = hrt_absolute_time(); - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub < 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } } @@ -780,7 +752,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -788,6 +759,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; if (_global_pos_pub < 0) { _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -799,19 +772,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* local position */ { + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_lat = lat; + hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; } float x; float y; - map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -899,8 +875,6 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a035..9ab84b58a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,9 +138,9 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; float _hil_local_alt0; + struct map_projection_reference_s _hil_local_proj_ref; }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9cb8e8344..6b0c44fb3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,7 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t rc_scale_yaw; + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -167,7 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float rc_scale_yaw; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; } _params; /** @@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update() { float v; - /* roll */ + /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); @@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; - /* pitch */ + /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); @@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; - /* yaw */ + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); @@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + /* manual control scale */ + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + + _params.man_roll_max *= M_PI / 180.0; + _params.man_pitch_max *= M_PI / 180.0; + _params.man_yaw_max *= M_PI / 180.0; return OK; } @@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() orb_check(_manual_control_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } @@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - - if (_manual_control_sp.yaw > 0.0f) { - yaw_sp_move_rate -= YAW_DEADZONE; - - } else { - yaw_sp_move_rate += YAW_DEADZONE; - } - - yaw_sp_move_rate *= _params.rc_scale_yaw; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + /* move yaw setpoint */ + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll; - _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d58..e52c39368 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b..65f4cbeaa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin <anton.babushkin@me.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +39,8 @@ * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -62,9 +63,10 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> @@ -114,20 +116,21 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _global_pos_sub; /**< vehicle local position */ + int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ - orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_global_position_s _global_pos; /**< vehicle global position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -148,9 +151,6 @@ private: param_t tilt_max; param_t land_speed; param_t land_tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; } _params_handles; /**< handles for interesting parameters */ struct { @@ -160,9 +160,6 @@ private: float land_speed; float land_tilt_max; - float rc_scale_pitch; - float rc_scale_roll; - math::Vector<3> pos_p; math::Vector<3> vel_p; math::Vector<3> vel_i; @@ -172,14 +169,15 @@ private: math::Vector<3> sp_offs_max; } _params; - double _lat_sp; - double _lon_sp; - float _alt_sp; + struct map_projection_reference_s _ref_pos; + float _ref_alt; + hrt_abstime _ref_timestamp; - bool _reset_lat_lon_sp; + bool _reset_pos_sp; bool _reset_alt_sp; - bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ + math::Vector<3> _pos; + math::Vector<3> _pos_sp; math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -202,9 +200,13 @@ private: static float scale_control(float ctl, float end, float dz); /** - * Reset lat/lon to current position + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position */ - void reset_lat_lon_sp(); + void reset_pos_sp(); /** * Reset altitude setpoint to current altitude @@ -252,31 +254,32 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _global_pos_sub(-1), + _local_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ _att_sp_pub(-1), - _pos_sp_triplet_pub(-1), + _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), - _lat_sp(0.0), - _lon_sp(0.0), - _alt_sp(0.0f), + _ref_alt(0.0f), + _ref_timestamp(0), - _reset_lat_lon_sp(true), - _reset_alt_sp(true), - _use_global_alt(false) + _reset_pos_sp(true), + _reset_alt_sp(true) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_global_pos, 0, sizeof(_global_pos)); + memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + memset(&_ref_pos, 0, sizeof(_ref_pos)); + _params.pos_p.zero(); _params.vel_p.zero(); _params.vel_i.zero(); @@ -285,6 +288,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); + _pos.zero(); + _pos_sp.zero(); _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -306,8 +311,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); - _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); /* fetch initial parameter values */ parameters_update(true); @@ -345,17 +348,18 @@ MulticopterPositionControl::parameters_update(bool force) orb_check(_params_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); + _params.tilt_max = math::radians(_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); - param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + _params.land_tilt_max = math::radians(_params.land_tilt_max); float v; param_get(_params_handles.xy_p, &v); @@ -402,33 +406,39 @@ MulticopterPositionControl::poll_subscriptions() orb_check(_att_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } orb_check(_att_sp_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } orb_check(_control_mode_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } orb_check(_manual_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } orb_check(_arming_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } - orb_check(_global_pos_sub, &updated); + orb_check(_local_pos_sub, &updated); - if (updated) - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + } } float @@ -452,40 +462,50 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void -MulticopterPositionControl::reset_lat_lon_sp() +MulticopterPositionControl::update_ref() { - if (_reset_lat_lon_sp) { - _reset_lat_lon_sp = false; - _lat_sp = _global_pos.lat; - _lon_sp = _global_pos.lon; - mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + if (_local_pos.ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos.ref_timestamp; } } void -MulticopterPositionControl::reset_alt_sp() +MulticopterPositionControl::reset_pos_sp() { - if (_reset_alt_sp) { - _reset_alt_sp = false; - _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + if (_reset_pos_sp) { + _reset_pos_sp = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } void -MulticopterPositionControl::select_alt(bool global) +MulticopterPositionControl::reset_alt_sp() { - if (global != _use_global_alt) { - _use_global_alt = global; - - if (global) { - /* switch from barometric to global altitude */ - _alt_sp += _global_pos.alt - _global_pos.baro_alt; - - } else { - /* switch from global to barometric altitude */ - _alt_sp += _global_pos.baro_alt - _global_pos.alt; - } + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } @@ -506,7 +526,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -537,8 +557,7 @@ MulticopterPositionControl::task_main() /* wakeup source */ struct pollfd fds[1]; - /* Setup of loop */ - fds[0].fd = _global_pos_sub; + fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -546,8 +565,9 @@ MulticopterPositionControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do */ if (pret < 0) { @@ -564,7 +584,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; @@ -572,28 +592,25 @@ MulticopterPositionControl::task_main() was_armed = _control_mode.flag_armed; + update_ref(); + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _vel(0) = _global_pos.vel_n; - _vel(1) = _global_pos.vel_e; - _vel(2) = _global_pos.vel_d; + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; - sp_move_rate.zero(); + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; - float alt = _global_pos.alt; + sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { - /* select altitude source and update setpoint */ - select_alt(_global_pos.global_valid); - - if (!_use_global_alt) { - alt = _global_pos.baro_alt; - } - /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ @@ -604,12 +621,12 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_position_enabled) { - /* reset lat/lon setpoint to current position if needed */ - reset_lat_lon_sp(); + /* reset position setpoint to current position if needed */ + reset_pos_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); - sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + sp_move_rate(0) = _manual.pitch; + sp_move_rate(1) = _manual.roll; } /* limit setpoint move rate */ @@ -625,74 +642,47 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); - _alt_sp -= sp_move_rate(2) * dt; + _pos_sp += sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); - pos_sp_offs(0) /= _params.sp_offs_max(0); - pos_sp_offs(1) /= _params.sp_offs_max(1); + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); - } - - /* fill position setpoint triplet */ - _pos_sp_triplet.previous.valid = true; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = true; - - _pos_sp_triplet.nav_state = NAV_STATE_NONE; - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - _pos_sp_triplet.current.lat = _lat_sp; - _pos_sp_triplet.current.lon = _lon_sp; - _pos_sp_triplet.current.alt = _alt_sp; - _pos_sp_triplet.current.yaw = _att_sp.yaw_body; - _pos_sp_triplet.current.loiter_radius = 0.0f; - _pos_sp_triplet.current.loiter_direction = 1.0f; - _pos_sp_triplet.current.pitch_min = 0.0f; - - /* publish position setpoint triplet */ - if (_pos_sp_triplet_pub > 0) { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); - - } else { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { - /* always use AMSL altitude for AUTO */ - select_alt(true); - /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; - /* update position setpoint */ - _lat_sp = _pos_sp_triplet.current.lat; - _lon_sp = _pos_sp_triplet.current.lon; - _alt_sp = _pos_sp_triplet.current.alt; + /* project setpoint to local frame */ + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { @@ -701,11 +691,25 @@ MulticopterPositionControl::task_main() } else { /* no waypoint, loiter, reset position setpoint if needed */ - reset_lat_lon_sp(); + reset_pos_sp(); reset_alt_sp(); } } + /* fill local position setpoint */ + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + _local_pos_sp.yaw = _att_sp.yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); @@ -729,9 +733,7 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err; - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - alt); + math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); @@ -741,7 +743,7 @@ MulticopterPositionControl::task_main() } if (!_control_mode.flag_control_position_enabled) { - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -847,8 +849,9 @@ MulticopterPositionControl::task_main() /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) + if (thr_min < 0.0f) { thr_min = 0.0f; + } } /* limit min lift */ @@ -939,8 +942,9 @@ MulticopterPositionControl::task_main() thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) + if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; + } } /* calculate attitude setpoint from thrust vector */ @@ -999,6 +1003,18 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode.flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; @@ -1021,7 +1037,7 @@ MulticopterPositionControl::task_main() } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; - _reset_lat_lon_sp = true; + _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -1060,18 +1076,21 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_pos_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (pos_control::g_control != nullptr) + if (pos_control::g_control != nullptr) { errx(1, "already running"); + } pos_control::g_control = new MulticopterPositionControl; - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != pos_control::g_control->start()) { delete pos_control::g_control; @@ -1083,8 +1102,9 @@ int mc_pos_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "not running"); + } delete pos_control::g_control; pos_control::g_control = nullptr; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 0082a5e6a..fe0004ea8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -100,6 +100,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -155,6 +156,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); * * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -176,15 +178,17 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); * * Limits maximum tilt in AUTO and EASY modes. * + * @unit deg * @min 0.0 - * @max 1.57 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); /** * Landing descend rate * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -195,8 +199,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); * * Limits maximum tilt on landing. * + * @unit deg * @min 0.0 - * @max 1.57 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 15.0f); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bcb3c8d0a..12fd35a0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -177,7 +177,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -690,84 +690,56 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; - - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } + /* publish position setpoint triplet on each status update if navigator active */ + _pos_sp_triplet_updated = true; - stick_mode = true; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + case NAV_STATE_MISSION: + request_mission_if_available(); + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; + case NAV_STATE_RTL: + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); } - } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - case NAV_STATE_MISSION: - request_mission_if_available(); - break; + break; - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - break; + break; - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); + } + } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); } } @@ -775,6 +747,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ @@ -813,17 +787,15 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each position update if navigator active */ _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -848,6 +820,8 @@ Navigator::task_main() } } + _global_pos_valid = _vstatus.condition_global_position_valid; + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -893,9 +867,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 763b87563..368424853 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file position_estimator_inav_main.c * Model-identification based position estimator for multirotors + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <unistd.h> @@ -57,6 +58,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/home_position.h> #include <uORB/topics/optical_flow.h> #include <mavlink/mavlink_log.h> #include <poll.h> @@ -95,8 +97,9 @@ static void usage(const char *reason); */ static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); @@ -112,8 +115,9 @@ static void usage(const char *reason) */ int position_estimator_inav_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { if (thread_running) { @@ -125,8 +129,9 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = false; if (argc > 1) - if (!strcmp(argv[2], "-v")) + if (!strcmp(argv[2], "-v")) { verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", @@ -163,16 +168,19 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +{ FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); fwrite(s, 1, n, f); n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } @@ -191,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + memset(x_est_prev, 0, sizeof(x_est_prev)); + memset(y_est_prev, 0, sizeof(y_est_prev)); + memset(z_est_prev, 0, sizeof(z_est_prev)); + int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted @@ -206,6 +219,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + struct map_projection_reference_s ref; + memset(&ref, 0, sizeof(ref)); + hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -238,7 +254,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; + static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation + float sonar_prev = 0.0f; + hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) hrt_abstime xy_src_time = 0; // time of last available position data @@ -257,6 +277,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + struct home_position_s home; + memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -274,10 +296,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; @@ -325,7 +348,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - global_pos.baro_valid = true; } } } @@ -425,6 +447,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* calculate time from previous update */ + float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; + flow_prev = flow.flow_timestamp; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -475,10 +501,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* convert raw flow to angular flow */ + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; + flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -503,8 +529,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy - if (!flow_accurate) + if (!flow_accurate) { w_flow *= 0.05f; + } flow_valid = true; @@ -516,32 +543,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } + /* home position */ + orb_check(home_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), home_position_sub, &home); + + if (home.timestamp != home_timestamp) { + home_timestamp = home.timestamp; + + double est_lat, est_lon; + float est_alt; + + if (ref_inited) { + /* calculate current estimated position in global frame */ + est_alt = local_pos.ref_alt - local_pos.z; + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + } + + /* update reference */ + map_projection_init(&ref, home.lat, home.lon); + + /* update baro offset */ + baro_offset += home.alt - local_pos.ref_alt; + + local_pos.ref_lat = home.lat; + local_pos.ref_lon = home.lon; + local_pos.ref_alt = home.alt; + local_pos.ref_timestamp = home.timestamp; + + if (ref_inited) { + /* reproject position estimate with new reference */ + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + z_est[0] = -(est_alt - local_pos.ref_alt); + } + + ref_inited = true; + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* hysteresis for GPS quality */ - if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { - gps_valid = false; - mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); - } + bool reset_est = false; - } else { - if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { - gps_valid = true; - mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); - } + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - gps_valid = false; + if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + gps_valid = true; + reset_est = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + } } if (gps_valid) { + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { @@ -549,18 +617,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_alt = alt + z_est[0]; + /* update baro offset */ + baro_offset -= z_est[0]; + + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ + x_est[0] = 0.0f; + x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; + y_est[0] = 0.0f; + y_est[1] = gps.vel_e_m_s; + z_est[0] = 0.0f; + y_est[2] = accel_NED[1]; + + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; + local_pos.ref_alt = alt; local_pos.ref_timestamp = t; /* initialize projection */ - map_projection_init(lat, lon); + map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } @@ -569,11 +644,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + + /* reset position estimate when GPS becomes good */ + if (reset_est) { + x_est[0] = gps_proj[0]; + x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; + y_est[0] = gps_proj[1]; + y_est[1] = gps.vel_e_m_s; + y_est[2] = accel_NED[1]; + } + /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { @@ -587,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); - w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } } else { @@ -704,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + } + /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - float x_est_prev[3], y_est_prev[3]; + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + corr_baro = 0; - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); + } else { + memcpy(z_est_prev, z_est, sizeof(z_est)); + } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -744,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); + + } else { + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); } } @@ -808,7 +909,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.xy_valid = can_estimate_xy && use_gps_xy; + local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; @@ -831,40 +932,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - /* publish global position */ - global_pos.global_valid = local_pos.xy_global; + if (local_pos.xy_global && local_pos.z_global) { + /* publish global position */ + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; - if (local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; global_pos.lon = est_lon; - global_pos.time_gps_usec = gps.time_gps_usec; - } + global_pos.alt = local_pos.ref_alt - local_pos.z; - /* set valid values even if position is not valid */ - if (local_pos.v_xy_valid) { global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; - } - - if (local_pos.z_global) { - global_pos.alt = local_pos.ref_alt - local_pos.z; - } - - if (local_pos.z_valid) { - global_pos.baro_alt = baro_offset - local_pos.z; - } - - if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; - } - global_pos.yaw = local_pos.yaw; + global_pos.yaw = local_pos.yaw; + + // TODO implement dead-reckoning + global_pos.eph = gps.eph_m; + global_pos.epv = gps.epv_m; - global_pos.timestamp = t; + if (vehicle_global_position_pub < 0) { + vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } else { + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + } } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index dcad5c03b..2e4f26661 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 611491cf9..b74d4183b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -960,6 +960,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -1101,8 +1102,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); @@ -1130,8 +1131,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + log_msg.body.log_GPOS.eph = buf.global_pos.eph; + log_msg.body.log_GPOS.epv = buf.global_pos.epv; LOGBUFFER_WRITE_AND_COUNT(GPOS); } @@ -1181,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.channel_count = buf.rc.chan_count; + log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 28d0706ff..595a787d6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -154,6 +154,7 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; + uint8_t failsafe_state; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -164,6 +165,7 @@ struct log_STAT_s { struct log_RC_s { float channel[8]; uint8_t channel_count; + uint8_t signal_lost; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -209,8 +211,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; - float baro_alt; - uint8_t flags; + float eph; + float epv; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -350,13 +352,13 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 14f7ac812..bc49f5c85 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); /** - * Mission switch channel mapping. + * Loiter switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); @@ -648,27 +648,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /** - * Roll scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); - -/** - * Pitch scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); - -/** - * Yaw scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - -/** * Failsafe channel PWM threshold. * * @min 800 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 214553d19..5315c5e38 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -135,7 +135,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -167,7 +167,15 @@ public: private: static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); /** * Gather and publish RC input data. @@ -246,7 +254,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -258,11 +266,6 @@ private: int rc_map_aux4; int rc_map_aux5; - float rc_scale_roll; - float rc_scale_pitch; - float rc_scale_yaw; - float rc_scale_flaps; - int32_t rc_fs_thr; float battery_voltage_scaling; @@ -294,7 +297,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -306,11 +309,6 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_scale_roll; - param_t rc_scale_pitch; - param_t rc_scale_yaw; - param_t rc_scale_flaps; - param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -434,8 +432,6 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _rc_last_valid(0), - _fd_adc(-1), _last_adc(0), @@ -470,6 +466,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -511,7 +508,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -521,11 +518,6 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); - _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); - /* RC failsafe */ _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); @@ -662,8 +654,8 @@ Sensors::parameters_update() warnx("%s", paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx("%s", paramerr); + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -679,10 +671,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); - param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); - param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); - param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ @@ -694,7 +682,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1267,6 +1255,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + } else { + return 0.0f; + } +} + +switch_pos_t +Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else if (value < -STICK_ON_OFF_LIMIT) { + return SWITCH_POS_OFF; + + } else { + return SWITCH_POS_MIDDLE; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1275,45 +1302,32 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct rc_input_values rc_input; + struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - if (rc_input.rc_lost) - return; - - struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; - - /* initialize to default values */ - manual_control.roll = NAN; - manual_control.pitch = NAN; - manual_control.yaw = NAN; - manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; - - manual_control.flaps = NAN; - manual_control.aux1 = NAN; - manual_control.aux2 = NAN; - manual_control.aux3 = NAN; - manual_control.aux4 = NAN; - manual_control.aux5 = NAN; - - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) { - return; - } + /* detect RC signal loss */ + bool signal_lost; - /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { - /* do not publish manual control setpoints when there are none */ - return; + /* check flags and require at least four channels to consider the signal valid */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check throttle failsafe */ + int8_t thr_ch = _rc.function[THROTTLE]; + if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { + /* throttle failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { + /* throttle failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } + } } unsigned channel_limit = rc_input.channel_count; @@ -1321,10 +1335,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1371,130 +1382,75 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp_last_signal; - - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } - - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); } else { - /* advertise the rc topic */ _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ - if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + if (!signal_lost) { + struct manual_control_setpoint_s manual; + memset(&manual, 0 , sizeof(manual)); + + /* fill values in manual_control_setpoint topic only if signal is valid */ + manual.timestamp = rc_input.timestamp_last_signal; + + /* limit controls */ + manual.roll = get_rc_value(ROLL, -1.0, 1.0); + manual.pitch = get_rc_value(PITCH, -1.0, 1.0); + manual.yaw = get_rc_value(YAW, -1.0, 1.0); + manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + + /* mode switches */ + manual.mode_switch = get_rc_switch_position(MODE); + manual.assisted_switch = get_rc_switch_position(ASSISTED); + manual.loiter_switch = get_rc_switch_position(LOITER); + manual.return_switch = get_rc_switch_position(RETURN); + + /* publish manual_control_setpoint topic */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); - } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - } + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + } - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + /* copy from mapped manual control to control group 3 */ + struct actuator_controls_s actuator_group_3; + memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + actuator_group_3.timestamp = rc_input.timestamp_last_signal; + + actuator_group_3.control[0] = manual.roll; + actuator_group_3.control[1] = manual.pitch; + actuator_group_3.control[2] = manual.yaw; + actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; + + /* publish actuator_controls_3 topic */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } - } void diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..a23d89cd2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -44,6 +44,16 @@ #include "../uORB.h" /** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + +/** * @addtogroup topics * @{ */ @@ -51,32 +61,25 @@ struct manual_control_setpoint_s { uint64_t timestamp; - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. */ - - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - - float flaps; /**< flap position */ - + float roll; /**< ailerons roll / roll rate input, -1..1 */ + float pitch; /**< elevator / pitch / pitch rate, -1..1 */ + float yaw; /**< rudder / yaw rate / yaw, -1..1 */ + float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ float aux2; /**< default function: camera pitch / tilt */ float aux3; /**< default function: camera trigger */ float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 98f0e3fa2..0196ae86b 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -57,6 +57,7 @@ struct optical_flow_s { uint64_t timestamp; /**< in microseconds since system start */ + uint64_t flow_timestamp; /**< timestamp from flow sensor */ int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 6eb20efd1..c168b2fac 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION { MODE = 4, RETURN = 5, ASSISTED = 6, - MISSION = 7, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, @@ -94,6 +94,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index adcd028f0..4897ca737 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -63,9 +63,6 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool global_valid; /**< true if position satisfies validity criteria of estimator */ - bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ @@ -74,8 +71,8 @@ struct vehicle_global_position_s { float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float eph; + float epv; }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index db9637cd9..a15303ea4 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,9 @@ /** * @file vehicle_local_position.h * Definition of the local fused NED position uORB topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ @@ -72,8 +74,8 @@ struct vehicle_local_position_s { bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ - int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ - int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + double ref_lat; /**< Reference point latitude in degrees */ + double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ /* Distance to surface */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 256c5134c..435230432 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -95,29 +95,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - MODE_SWITCH_AUTO -} mode_switch_pos_t; - -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_EASY -} assisted_switch_pos_t; - -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -189,11 +166,6 @@ struct vehicle_status_s { bool is_rotary_wing; - mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; - assisted_switch_pos_t assisted_switch; - mission_switch_pos_t mission_switch; - bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; |