From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 22:44:05 +0100 Subject: [PATCH 001/320] ACRO mode implemented --- src/modules/commander/commander.cpp | 35 +++++- .../commander/state_machine_helper.cpp | 4 + .../mc_att_control/mc_att_control_main.cpp | 102 +++++++++++++----- .../mc_att_control/mc_att_control_params.c | 3 + src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++ .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/rc_channels.h | 11 +- src/modules/uORB/topics/vehicle_status.h | 8 ++ 9 files changed, 144 insertions(+), 35 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..89294fc76c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; - main_states_str[3] = "AUTO"; + main_states_str[1] = "ACRO"; + main_states_str[2] = "SEATBELT"; + main_states_str[3] = "EASY"; + main_states_str[4] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* acro switch */ + if (!isfinite(sp_man->acro_switch)) { + status->acro_switch = ACRO_SWITCH_NONE; + + } else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) { + status->acro_switch = ACRO_SWITCH_ACRO; + + } else { + status->acro_switch = ACRO_SWITCH_NORMAL; + } } transition_result_t @@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status) switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (status->acro_switch == ACRO_SWITCH_ACRO) { + res = main_state_transition(status, MAIN_STATE_ACRO); + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1600,6 +1616,17 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..fd966a0682 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -230,6 +230,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ 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 a0accb8552..5f862652a3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -161,7 +161,12 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t roll_scale_acro; + param_t pitch_scale_acro; + param_t yaw_scale_acro; + param_t rc_scale_roll; + param_t rc_scale_pitch; param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ @@ -171,8 +176,9 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + math::Vector<3> scale_acro; /**< scale for ACRO mode */ - float rc_scale_yaw; + math::Vector<3> rc_scale; /**< scale for MANUAL mode */ } _params; /** @@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.scale_acro.zero(); + _params.rc_scale.zero(); _R_sp.identity(); _R.identity(); @@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO"); + _params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO"); + _params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_scale_acro, &v); + _params.scale_acro(0) = v; + param_get(_params_handles.rc_scale_roll, &v); + _params.rc_scale(0) = v; /* pitch */ param_get(_params_handles.pitch_p, &v); @@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_scale_acro, &v); + _params.scale_acro(1) = v; + param_get(_params_handles.rc_scale_pitch, &v); + _params.rc_scale(1) = v; /* yaw */ param_get(_params_handles.yaw_p, &v); @@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + param_get(_params_handles.yaw_scale_acro, &v); + _params.scale_acro(2) = v; + param_get(_params_handles.rc_scale_yaw, &v); + _params.rc_scale(2) = v; return OK; } @@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ + vehicle_manual_poll(); if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ @@ -488,16 +511,16 @@ 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) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2); + if (_params.rc_scale(2) > 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; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2); 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; + yaw_sp_move_rate *= _params.rc_scale(2); _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; @@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main() parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control, ACRO mode */ + vehicle_manual_poll(); + + _rates_sp(0) = _manual_control_sp.roll; + _rates_sp(1) = _manual_control_sp.pitch; + _rates_sp(2) = _manual_control_sp.yaw; + + /* rescale controls for ACRO mode */ + _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); + _thrust_sp = _manual_control_sp.throttle; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } } if (_v_control_mode.flag_control_rates_enabled) { 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 27a45b6bbc..f3a4022c84 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a0..e72b48a881 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bd665b592b..50ddec8a94 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,6 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_acro_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,6 +297,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_acro_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -515,6 +517,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_acro_sw = param_find("RC_MAP_ACRO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -669,6 +672,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("Failed getting acro sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -700,6 +707,7 @@ Sensors::parameters_update() _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[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1290,6 +1298,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.acro_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1428,6 +1437,11 @@ Sensors::rc_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* acro switch input */ + if (_rc.function[ACRO] >= 0) { + manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled); + } + /* return switch input */ if (_rc.function[RETURN] >= 0) { manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..190dc01c8e 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,6 +60,7 @@ struct manual_control_setpoint_s { 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 */ + float acro_switch; /**< acro 2 position switch (optional): normal, acro */ /** * Any of the channels below may not be available and be set to NaN diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef150..beb7008ab9 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION ASSISTED = 6, MISSION = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1b3639e309..5cb0bd8a20 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,6 +63,7 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, + MAIN_STATE_ACRO, MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, @@ -116,6 +117,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + ACRO_SWITCH_NONE = 0, + ACRO_SWITCH_NORMAL, + ACRO_SWITCH_ACRO +} acro_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -192,6 +199,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + acro_switch_pos_t acro_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:25:29 +0100 Subject: [PATCH 002/320] commander: allow disarming in ACRO without landing (as in MANUAL) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89294fc76c..30f75b9fb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ 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) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { From 21300874ac6d337d85c49704e9233fdd17192171 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 23:07:48 +0100 Subject: [PATCH 003/320] mc_att_control: reset yaw setpoint after ACRO --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 21e1016625..77531cbb92 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -802,6 +802,8 @@ MulticopterAttitudeControl::task_main() _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); _thrust_sp = _manual_control_sp.throttle; + _reset_yaw_sp = true; + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); From 6e7136c6b3f953c26cf75c9f8b777a6a7c84ea9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:50:26 +0100 Subject: [PATCH 004/320] rc_channels topic: bug fixed; sensors: minor cleanup --- src/modules/sensors/sensors.cpp | 2 +- src/modules/uORB/topics/rc_channels.h | 34 +++++++++++++-------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b37a744ca2..73f14225de 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -674,7 +674,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { - warnx("Failed getting acro sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index beb7008ab9..36106751e2 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -45,12 +45,12 @@ /** * The number of RC channel inputs supported. * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 15. + * leaving at a sane value of 16. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAPPED_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 16 /** * This defines the mapping of the RC functions. @@ -60,21 +60,21 @@ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - ACRO = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 14, + ROLL, + PITCH, + YAW, + MODE, + RETURN, + ASSISTED, + MISSION, + OFFBOARD_MODE, + ACRO, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:55:12 +0100 Subject: [PATCH 005/320] mavlink: custom mode ACRO added --- src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9c..d69ab30677 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f7..22465a3dae 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; From 9315796020339906d30580892f57abcdc1238b0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 07:55:22 +0100 Subject: [PATCH 006/320] Enable S.BUS TX pin --- nuttx-configs/px4io-v2/include/board.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 4b9c906385..17e77918b4 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -103,8 +103,6 @@ #define GPIO_USART2_RTS 0xffffffff #undef GPIO_USART2_CK #define GPIO_USART2_CK 0xffffffff -#undef GPIO_USART3_TX -#define GPIO_USART3_TX 0xffffffff #undef GPIO_USART3_CK #define GPIO_USART3_CK 0xffffffff #undef GPIO_USART3_CTS From 500ac69ee46ad582eee5a4321bd53665e17032da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:13:53 +0100 Subject: [PATCH 007/320] Build test code for S.BUS output, send test characters once S.BUS1 or S.BUS2 is enabled --- src/modules/px4iofirmware/mixer.cpp | 14 ++++++++++++++ src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/sbus.c | 14 +++++++++++++- 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec6..3eaecc38b9 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,10 +254,24 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bb224f3880..0e9fee8aeb 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -218,6 +218,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus1_output(uint16_t *values, uint16_t num_values); +extern bool sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index f6ec542eb0..86240d36ab 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -93,7 +93,7 @@ int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -117,6 +117,18 @@ sbus_init(const char *device) return sbus_fd; } +bool +sbus1_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'A', 1); +} + +bool +sbus2_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'B', 1); +} + bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { From 85ec7fb40aab728ba477ffd75f48c2c731fb56fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:47:01 +0100 Subject: [PATCH 008/320] test loop --- src/modules/px4iofirmware/sbus.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 86240d36ab..39230d274c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,6 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } + ENABLE_SBUS_OUT(true); + + while (1) { + const char* hello = 'HELLO WORLD'; + if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) + break; + } + return sbus_fd; } From 3bcf34098a9fd07c0693e918396d2e3fde0fa1e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:50:44 +0100 Subject: [PATCH 009/320] Fix quotation marks --- src/modules/px4iofirmware/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 39230d274c..6608392d04 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -117,7 +117,7 @@ sbus_init(const char *device) ENABLE_SBUS_OUT(true); while (1) { - const char* hello = 'HELLO WORLD'; + const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; } From bc3f95fc07fb01edecfa9147023a354f1f237e92 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:15 -0800 Subject: [PATCH 010/320] Turn off JTAG completely in a vain attempt to get PB4 free for SBUS enable. --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 4111e70eba..2c63b88190 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -104,7 +104,7 @@ CONFIG_ARMV7M_CMNVECTOR=y # CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled # CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n CONFIG_STM32_JTAG_SW_ENABLE=n From ca2ad0051d2c5a31aa6050cc88f5c7d6c2997036 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:53 -0800 Subject: [PATCH 011/320] Be more enthusiastic with the sbus enable pin. Still no love. --- src/modules/px4iofirmware/sbus.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6608392d04..32be93d4c9 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,12 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } - ENABLE_SBUS_OUT(true); + stm32_configgpio(GPIO_SBUS_OENABLE); while (1) { + ENABLE_SBUS_OUT(true); const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; + ENABLE_SBUS_OUT(false); } return sbus_fd; From cec6b8925e727710884042f9b45c105aa8c4d5af Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:10:45 -0800 Subject: [PATCH 012/320] Don't leave all JTAG off... it will make you very sad --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 2c63b88190..e6563e5878 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_ARMV7M_CMNVECTOR=y CONFIG_STM32_DFU=n CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # # Individual subsystems can be enabled: From dd432e66032c3cb1cb6f65536c28af1dd9f97317 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:11:09 -0800 Subject: [PATCH 013/320] Remove the s.bus test loop... makes it very hard to update the firmware. --- src/modules/px4iofirmware/sbus.c | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32be93d4c9..0e7dc621cf 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -113,17 +113,6 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } - - stm32_configgpio(GPIO_SBUS_OENABLE); - - while (1) { - ENABLE_SBUS_OUT(true); - const char* hello = "HELLO WORLD"; - if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) - break; - ENABLE_SBUS_OUT(false); - } - return sbus_fd; } From 6a1f91e6254e14c52b77406b12b76e2a233aedf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:22:05 +0100 Subject: [PATCH 014/320] Make SBUS output exclusive --- src/modules/px4iofirmware/mixer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3eaecc38b9..b175c3bc8c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -255,11 +255,12 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + } } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ From 91c55503a860ffc02a2687c141e2cfc68a43b3cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:25:49 +0100 Subject: [PATCH 015/320] Ensure only either S.BUS1 or S.BUS2 can be active at a time --- src/modules/px4iofirmware/registers.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e13..f780868398 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -462,9 +462,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - /* disable the conflicting options */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: [PATCH 016/320] mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 7 +-- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217de..41670e2474 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a2966..819dcf9c3a 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,14 +59,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +76,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c572972b58..11181ff642 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: [PATCH 017/320] mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e2474..b41e9d355d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: [PATCH 018/320] mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355d..02e35f1a8b 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) From 40394c6744765d9383b5c2aa6d32162b32bda567 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 11 Mar 2014 10:40:08 +0100 Subject: [PATCH 019/320] navigator: RTL: set loiter wp instead of normal waypoint in RTL_STATE_DESCEND --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 16eca8d791..6b6f3f3ff3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1375,7 +1375,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; From e8efbc7f3fe22d2a11189fd83c7252347a6d7f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 11 Mar 2014 10:37:15 +0100 Subject: [PATCH 020/320] navigator: RTL: set normal waypoint instead of takeoff waypoint in RTL_STATE_CLIMB --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6b6f3f3ff3..2f058b6c92 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1315,7 +1315,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; From 3317259e798b31ff407e07188f3080f6dbccf478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 11:56:04 +0100 Subject: [PATCH 021/320] integrate range finder into fw landing --- .../fw_pos_control_l1_main.cpp | 90 +++++++++++++++---- .../fw_pos_control_l1_params.c | 10 +++ .../fw_pos_control_l1/landingslope.cpp | 34 +++++-- src/modules/fw_pos_control_l1/landingslope.h | 31 ++++++- 4 files changed, 138 insertions(+), 27 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df7854..32e00659bc 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -89,6 +89,7 @@ #include #include #include +#include #include "landingslope.h" @@ -134,6 +135,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ + int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -147,6 +149,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -181,7 +184,7 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_last; + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; @@ -239,6 +242,7 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -283,6 +287,7 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -308,6 +313,12 @@ private: */ bool vehicle_airspeed_poll(); + /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + /** * Check for position updates. */ @@ -328,6 +339,11 @@ private: */ void navigation_capabilities_publish(); + /** + * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + */ + float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + /** * Control position. */ @@ -384,6 +400,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -403,7 +420,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), last_manual(false), usePreTakeoffThrust(false), - flare_curve_alt_last(0.0f), + flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), @@ -417,7 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -442,6 +460,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -531,6 +550,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -626,6 +647,20 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::range_finder_poll() +{ + /* check if there is a range finder measurement */ + bool range_finder_updated; + orb_check(_range_finder_sub, &range_finder_updated); + + if (range_finder_updated) { + orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); + } + + return range_finder_updated; +} + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -754,6 +789,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +{ + float rel_alt_estimated = current_alt - land_setpoint_alt; + + /* only use range finder if: + * parameter (range_finder_use_relative_alt) > 0 + * the measurement is valid + * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt + */ + if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + return rel_alt_estimated; + } + + return range_finder.distance; + +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -896,12 +948,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); + + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -911,7 +965,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -920,16 +974,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.alt; + flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -941,7 +995,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - flare_curve_alt_last = flare_curve_alt; + flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: @@ -949,20 +1003,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ - altitude_desired = landing_slope_alt_desired; + altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); + altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1185,6 +1239,7 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1264,6 +1319,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 0909135e15..8ec0f0027d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -375,3 +375,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Landing relative altitude threshold for range finder measurements + * + * range finder measurements will only be used below FW_LND_RFRALT estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) + * set to -1 to disable + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, 30.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index e5f7023ae1..8ce465fe8c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + return getLandingSlopeRelativeAltitude(wp_landing_distance); + else + return 0.0f; +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude) +{ + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +{ + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); else return wp_altitude; } -float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; else - return wp_landing_altitude; + return 0.0f; +} + +float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + + return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 76d65a55f2..b54fd501cf 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,11 +63,26 @@ public: Landingslope() {} ~Landingslope() {} + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeRelativeAltitude(float wp_landing_distance); + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); + + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude); /** * @@ -76,13 +91,22 @@ public: */ float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); + /** + * + * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative + } + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; } /** @@ -95,8 +119,9 @@ public: } + float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); + float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, From b70008242bf6959387958ea1b34a5e6ad89bfe27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 15:28:37 +0100 Subject: [PATCH 022/320] add missing $ in rcS script --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 55e2a886e1..7c3524fef0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -466,7 +466,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -525,7 +525,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.mc_apps fi From 37d2cff83d3ecd697afb13a991b3c96133178318 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:03:26 +0100 Subject: [PATCH 023/320] set range finder to disabled as default --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 8ec0f0027d..9e2402447a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -384,4 +384,4 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); From 5894d72aa8077eae559f4444adb9203d59754f5f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:39:18 +0100 Subject: [PATCH 024/320] fw landing: do not use relative altitudes in TECS --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 32e00659bc..244b82ee1e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -983,7 +983,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -1016,7 +1016,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, altitude_desired_rel, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 3074bced5666934abf348c9fb2fc4bd4f4b6ca57 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:59:20 +0100 Subject: [PATCH 025/320] fix comment for FW_LND_RFRALT --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9e2402447a..534e8d110d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -377,10 +377,11 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Landing relative altitude threshold for range finder measurements + * Relative altitude threshold for range finder measurements for use during landing * - * range finder measurements will only be used below FW_LND_RFRALT estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) - * set to -1 to disable + * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT + * set to < 0 to disable + * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location * * @group L1 Control */ From 1362d5f195bc02f8f9c3ad0988768d547c705748 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 30 Mar 2014 20:37:28 +0400 Subject: [PATCH 026/320] px4fmu: support all actuator control groups, dynamically subscribe to required topics --- src/drivers/px4fmu/fmu.cpp | 264 ++++++++++++++++++++++--------------- 1 file changed, 161 insertions(+), 103 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c5..9a1da39cbe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,19 +120,25 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; - int _t_actuators; - int _t_actuator_armed; - orb_advert_t _t_outputs; + int _armed_sub; + orb_advert_t _outputs_pub; + actuator_armed_s _armed; unsigned _num_outputs; bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + bool _servo_armed; bool _pwm_on; MixerGroup *_mixers; - actuator_controls_s _controls; + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; @@ -149,7 +155,7 @@ private: uint8_t control_group, uint8_t control_index, float &input); - + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -216,15 +222,17 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _t_outputs(0), + _control_subs({-1}), + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(-1), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), + _servo_armed(false), _pwm_on(false), _mixers(nullptr), + _groups_required(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -235,6 +243,14 @@ PX4FMU::PX4FMU() : _max_pwm[i] = PWM_DEFAULT_MAX; } + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + _debug_enabled = true; } @@ -447,33 +463,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); } +void +PX4FMU::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + void PX4FMU::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL // rc input, published to ORB @@ -491,6 +517,12 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } /* * Adjust actuator topic update rate to keep up with @@ -515,7 +547,11 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; @@ -523,7 +559,7 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); + int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { @@ -537,90 +573,99 @@ PX4FMU::task_main() } else { - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_6PWM: - num_outputs = 6; - break; - - default: - num_outputs = 0; - break; + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - } - - uint16_t pwm_limited[num_outputs]; - - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + poll_id++; } } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = _armed.armed && !_armed.lockdown; - if (_armed != set_armed) - _armed = set_armed; + if (_servo_armed != set_armed) + _servo_armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); } } + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* publish mixed control outputs */ + if (_outputs_pub < 0) { + _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + } else { + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); + } + } } #ifdef HRT_PPM_CHANNEL @@ -661,8 +706,13 @@ PX4FMU::task_main() } - ::close(_t_actuators); - ::close(_t_actuator_armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -684,7 +734,7 @@ PX4FMU::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -1052,6 +1102,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -1060,18 +1111,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + (uintptr_t)_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -1082,9 +1135,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -1095,7 +1149,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); } } From db37d3a4959c6f0888708bae4b4efd66c668e5b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 13:36:55 +0400 Subject: [PATCH 027/320] fmu driver: bugs fixed --- src/drivers/px4fmu/fmu.cpp | 45 +++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9a1da39cbe..8aa4473d41 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -233,6 +233,7 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -584,28 +585,6 @@ PX4FMU::task_main() } } - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; - - if (_servo_armed != set_armed) - _servo_armed = set_armed; - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } - } - /* can we mix? */ if (_mixers != nullptr) { @@ -668,6 +647,28 @@ PX4FMU::task_main() } } + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + if (_servo_armed != set_armed) + _servo_armed = set_armed; + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } + } + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data From 0594343b9ccbb964808469041a1356414e281cbf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 12 Apr 2014 16:13:34 +0200 Subject: [PATCH 028/320] QU4D startup script --- .../init.d/10017_steadidrone_qu4d | 35 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ 2 files changed, 40 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d new file mode 100644 index 0000000000..5728a09205 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -0,0 +1,35 @@ +#!nsh +# +# Steadidrone QU4D +# +# Thomas Gubler +# Lorenz Meier +# + +sh /etc/init.d/rc.mc_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + # TODO tune roll/pitch separately + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 0.5 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set BAT_N_CELLS 4 +fi + +set MIXER FMU_quad_w + +set PWM_MIN 1210 +set PWM_MAX 1900 + +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d393..b365bd6420 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -195,6 +195,11 @@ then sh /etc/init.d/10016_3dr_iris fi +if param compare SYS_AUTOSTART 10017 +then + sh /etc/init.d/10017_steadidrone_qu4d +fi + # # Hexa Coaxial # From e2989bc091c1a4e287534947095a1777d689ddfa Mon Sep 17 00:00:00 2001 From: eneadev Date: Sat, 19 Apr 2014 13:08:29 +0200 Subject: [PATCH 029/320] Update drv_hrt.c TIM3 was not working properly with a custom application and I got it work changing TIM3 HRT_TIMER_POWER_BIT from RCC_APB2ENR_TIM3EN to RCC_APB1ENR_TIM3EN because on datasheet TIM3 is on APB1, I think that you should check also the others --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index b7c9b89a45..f324b341e1 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -94,7 +94,7 @@ #elif HRT_TIMER == 3 # define HRT_TIMER_BASE STM32_TIM3_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM3 # define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN # if CONFIG_STM32_TIM3 From edd16afead95bbf236d974ad895e10cc2ef70033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 15:49:29 +0200 Subject: [PATCH 030/320] Add filter parameters and multicopter defaults to parametrize Pauls estimator correctly when running for multicopters. Estimator itself not updated yet, will be next step. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 6 + .../fw_att_pos_estimator_main.cpp | 44 ++++++ .../fw_att_pos_estimator_params.c | 131 ++++++++++++++++++ 3 files changed, 181 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a9..c1f9db7d1c 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/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585ec..96db3f20cc 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -199,6 +199,17 @@ 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; } _parameters; /**< local copies of interesting parameters */ struct { @@ -207,6 +218,17 @@ 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; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -302,6 +324,17 @@ FixedwingEstimator::FixedwingEstimator() : _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"); /* fetch initial parameter values */ parameters_update(); @@ -366,6 +399,17 @@ 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)); return OK; } 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 index 6138ef39cd..9d01a095c8 100644 --- 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 @@ -115,3 +115,134 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); */ PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); +/** + * Velocity 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); + From 7b61c927f0420cb3b519972221654176e7c9274b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 16:13:13 +0200 Subject: [PATCH 031/320] Renamed FW filter to EKF to express its generic properties, switched multicopters over to this filter for first tests. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 3 ++- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../estimator.cpp | 0 .../estimator.h | 0 .../fw_att_pos_estimator_main.cpp | 6 +++--- .../fw_att_pos_estimator_params.c | 0 .../module.mk | 2 +- 9 files changed, 9 insertions(+), 8 deletions(-) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.cpp (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.h (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_main.cpp (99%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_params.c (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/module.mk (97%) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec8..9aca3fc5f4 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 ed3939757a..c75281fcdd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,7 +5,8 @@ # attitude_estimator_ekf start -position_estimator_inav start +ekf_att_pos_estimator start +#position_estimator_inav start mc_att_control start mc_pos_control start diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..1daf8277ea 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 e13421acc5..7f0c59515a 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/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.cpp rename to src/modules/ekf_att_pos_estimator/estimator.cpp diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.h rename to src/modules/ekf_att_pos_estimator/estimator.h 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 similarity index 99% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 96db3f20cc..7857a04693 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 @@ -90,7 +90,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(); @@ -1255,10 +1255,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/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c similarity index 100% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk similarity index 97% rename from src/modules/fw_att_pos_estimator/module.mk rename to src/modules/ekf_att_pos_estimator/module.mk index c992959e0d..30955d0ddc 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 \ From 86a0862af6412906611ed295cae4604e7111b1e9 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sat, 19 Apr 2014 13:07:09 -0700 Subject: [PATCH 032/320] Added rc_map_failsafe to enable use of channels other than throttle for failsafe. --- src/modules/sensors/sensor_params.c | 29 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 19 +++++++++++-------- 2 files changed, 40 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 14f7ac812f..ff121c51ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -535,6 +535,35 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +/** + * Failsafe channel mapping. + * + * The RC mapping index indicates which rc function + * should be used for detecting the failsafe condition + * + * @min 0 + * @max 14 + * + * mapping (from rc_channels.h) + * THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + * + */ +PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function + /** * Throttle control channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4083b8b4f5..fa3905f608 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -242,6 +242,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -290,6 +291,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -512,6 +514,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_failsafe = param_find("RC_MAP_FAILSAFE"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,6 +653,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -704,7 +711,6 @@ Sensors::parameters_update() _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -1310,8 +1316,8 @@ Sensors::rc_poll() } /* 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))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.min[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.max[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } @@ -1434,7 +1440,8 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { + + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } @@ -1455,10 +1462,6 @@ Sensors::rc_poll() 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; From dca1e7fc611bb44caf1fc586e45105d170955de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 22:40:37 +0200 Subject: [PATCH 033/320] Decomission unmaintained position estimator --- .../att_pos_estimator_ekf/KalmanNav.cpp | 815 ------------------ .../att_pos_estimator_ekf/KalmanNav.hpp | 192 ----- .../att_pos_estimator_ekf/kalman_main.cpp | 157 ---- src/modules/att_pos_estimator_ekf/module.mk | 42 - src/modules/att_pos_estimator_ekf/params.c | 49 -- 5 files changed, 1255 deletions(-) delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp delete mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/module.mk delete mode 100644 src/modules/att_pos_estimator_ekf/params.c 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 668bac5d9c..0000000000 --- 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 - -#include "KalmanNav.hpp" -#include -#include - -// 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 caf93bc787..0000000000 --- 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 - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -/** - * 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 _sensors; /**< sensors sub. */ - uORB::Subscription _gps; /**< gps sub. */ - uORB::Subscription _param_update; /**< parameter update sub. */ - // publications - uORB::Publication _pos; /**< position pub. */ - uORB::Publication _localPos; /**< local position pub. */ - uORB::Publication _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 3d20d4d2d6..0000000000 --- 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#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 ]"); - 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 8d4a40d950..0000000000 --- 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 4af5edeade..0000000000 --- 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 - -/*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); From ce56d75bc606015728f59a3e811fa48ff9db2979 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 01:37:31 +0200 Subject: [PATCH 034/320] Updated filter to most recent version with accel scale estimation, exposed crucial parameters for cross-vehicle support --- .../ekf_att_pos_estimator/estimator.cpp | 1610 +++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 120 +- .../fw_att_pos_estimator_main.cpp | 25 +- .../fw_att_pos_estimator_params.c | 14 +- 4 files changed, 1188 insertions(+), 581 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index c312173938..ebe30cae0e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,5 @@ #include "estimator.h" - +//#include #include float Vector3f::length(void) const @@ -69,6 +69,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -109,7 +128,6 @@ void swap_var(float &d1, float &d2) AttPosEKF::AttPosEKF() : fusionModeGPS(0), covSkipCount(0), - EAS2TAS(1.0f), statesInitialised(false), fuseVelData(false), fusePosData(false), @@ -120,10 +138,11 @@ AttPosEKF::AttPosEKF() : staticMode(true), useAirspeed(true), useCompass(true), + useRangeFinder(true), numericalProtection(true), storeIndex(0) { - + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -143,8 +162,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float q12; float q13; float q23; - Mat3f Tbn; - Mat3f Tnb; float rotationMag; float qUpdated[4]; float quatMag; @@ -157,7 +174,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; + dVelIMU.z = dVelIMU.z - states[13]; // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -177,8 +194,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - float rotScaler = (sin(0.5f*rotationMag))/rotationMag; + 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; @@ -260,11 +277,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() void AttPosEKF::CovariancePrediction(float dt) { // scalars - float windVelSigma; - float dAngBiasSigma; - // float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; float daxCov; float dayCov; float dazCov; @@ -284,28 +296,29 @@ void AttPosEKF::CovariancePrediction(float dt) float dax_b; float day_b; float daz_b; + float dvz_b; // arrays - float processNoise[21]; - float SF[14]; + float processNoise[n_states]; + float SF[15]; float SG[8]; float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; // 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]); + 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; @@ -321,29 +334,33 @@ void AttPosEKF::CovariancePrediction(float dt) 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); + 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); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); // 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; + 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); @@ -366,169 +383,183 @@ void AttPosEKF::CovariancePrediction(float dt) 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]; + 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[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[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); @@ -543,13 +574,15 @@ void AttPosEKF::CovariancePrediction(float 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[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); @@ -564,13 +597,15 @@ void AttPosEKF::CovariancePrediction(float 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[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); @@ -585,13 +620,15 @@ void AttPosEKF::CovariancePrediction(float 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[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; @@ -606,13 +643,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -627,13 +666,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -648,13 +689,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -669,13 +712,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -690,13 +735,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -711,13 +758,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -732,13 +781,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -753,13 +804,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -774,13 +827,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -795,13 +850,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[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; @@ -816,6 +873,54 @@ void AttPosEKF::CovariancePrediction(float dt) 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++) { @@ -826,16 +931,24 @@ void AttPosEKF::CovariancePrediction(float dt) // setting the coresponding covariance terms to zero. if (onGround || !useCompass) { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); + 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,13,14); - zeroCols(nextP,13,14); + 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 @@ -857,7 +970,7 @@ void AttPosEKF::CovariancePrediction(float dt) if (onGround || staticMode) { // copy the portion of the variances we want to // propagate - for (unsigned i = 0; i < 14; i++) { + for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; // force symmetry for observable states @@ -866,7 +979,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t j = 0; j < i; j++) { - if ((i > 12) || (j > 12)) { + if ((i > 13) || (j > 13)) { P[i][j] = 0.0f; } else { P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); @@ -946,12 +1059,12 @@ void AttPosEKF::FuseVelposNED() // 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[0] = sq(vneSigma) + 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; + 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++) @@ -1046,11 +1159,11 @@ void AttPosEKF::FuseVelposNED() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) @@ -1080,6 +1193,12 @@ void AttPosEKF::FuseVelposNED() { 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++) { @@ -1117,24 +1236,38 @@ void AttPosEKF::FuseVelposNED() 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; + static uint8_t obsIndex; + static float MagPred[3]; + static float SH_MAG[9]; + 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 magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + float R_MAG = sq(magMeasurementSigma); 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}; + 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 @@ -1147,46 +1280,30 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } - 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]; + 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 @@ -1204,7 +1321,7 @@ void AttPosEKF::FuseMagnetometer() 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); + 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; @@ -1217,135 +1334,145 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + 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[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; + 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[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[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][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]); + 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 1e-12f) { - for (uint8_t j= 0; j<=3; j++) + for (uint8_t j= 0; j <= 3; j++) { float quatMagInv = 1.0f/quatMag; states[j] = states[j] * quatMagInv; @@ -1513,38 +1642,38 @@ void AttPosEKF::FuseAirspeed() // 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 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++) + 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++) + 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 = 15; j<=20; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) + 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++) + 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<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1556,6 +1685,343 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +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::FuseOpticalFlow() +{ + static uint8_t obsIndex; + static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[17]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float R_LOS = 0.01f; + // Transformation matrix from body to navigation axes + Mat3f DCM; + // Transformation matrix from sensor to body axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tsb; + Tsb.x.y = -a3; + Tsb.y.x = a3; + Tsb.x.z = a2; + Tsb.z.x = -a2; + Tsb.y.z = -a1; + Tsb.z.y = a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptData || obsIndex == 1) && !onGround && Tbn.z.z > 0.866f) + { + // Sequential fusion of XY components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptData) + { + // Copy required states to local variable names + q0 = statesAtLosMeasTime[0]; + q1 = statesAtLosMeasTime[1]; + q2 = statesAtLosMeasTime[2]; + q3 = statesAtLosMeasTime[3]; + vn = statesAtLosMeasTime[4]; + ve = statesAtLosMeasTime[5]; + vd = statesAtLosMeasTime[6]; + pd = statesAtLosMeasTime[9]; + ptd = statesAtLosMeasTime[2]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + DCM.x.x = q0 + q1 - q2 - q3; + DCM.y.y = q0 - q1 + q2 - q3; + DCM.z.z = q0 - q1 - q2 + q3; + DCM.x.y = 2*(q12 - q03); + DCM.x.z = 2*(q13 + q02); + DCM.y.x = 2*(q12 + q03); + DCM.y.z = 2*(q23 - q01); + DCM.z.x = 2*(q13 - q02); + DCM.z.y = 2*(q23 + q01); + // calculate transformation from NED to sensor axes + Tns = DCM*Tsb; + Tns = Tns.transpose(); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ((ptd - pd)/Tns.z.z); + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + // divide by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // scale optical flow observation error with total angular rate + R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); + SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); + SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SK_LOS[16] = SH_LOS[0]; + + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + varInnovLOS[0] = 1.0f/SK_LOS[0]; + innovLOS[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + varInnovLOS[1] = 1.0f/SK_LOS[1]; + innovLOS[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1619,51 +2085,54 @@ void AttPosEKF::ResetStoredStates() } // 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 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++) - // { - // // 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; - // } + 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 (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 (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; } @@ -1746,7 +2215,7 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +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); @@ -1783,23 +2252,25 @@ void AttPosEKF::CovarianceInit() 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[4][4] = sq(0.7f); P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); + 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(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]; + 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) @@ -1818,45 +2289,52 @@ void AttPosEKF::ConstrainVariances() // 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) + // 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 < 4; i++) { + for (unsigned i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { + // 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 < 10; i++) { + for (unsigned i = 7; i <= 9; 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)); + // 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 = 13; i < 15; i++) { + 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 = 15; i < 18; i++) { + 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 = 18; i < 21; i++) { + 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() @@ -1870,23 +2348,24 @@ void AttPosEKF::ConstrainStates() // 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) - + // 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 < 4; i++) { + 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 < 7; i++) { + 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 < 9; i++) { + for (unsigned i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -1894,26 +2373,32 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { + 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 = 13; i < 15; i++) { + 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 = 15; i < 18; i++) { + 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 = 18; i < 21; i++) { + 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() @@ -2001,7 +2486,7 @@ void AttPosEKF::ResetVelocity(void) void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { - for (int i = 0; i < n_states; i++) + for (unsigned i = 0; i < n_states; i++) { err->states[i] = states[i]; } @@ -2173,14 +2658,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // 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 + 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 statesInitialised = true; @@ -2239,10 +2725,10 @@ void AttPosEKF::ZeroVariables() void AttPosEKF::GetFilterState(struct ekf_status_report *state) { - memcpy(state, ¤t_ekf_state, sizeof(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)); + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e62f1a98a5..6effe062d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -45,14 +45,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 +77,79 @@ 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 a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + + 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. + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) + + 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; + } + + + + // 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 +164,8 @@ 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 + float statesAtLosMeasTime[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 +174,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 +189,30 @@ 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 + float innovMag[3]; // innovation output for magnetometer measurements + float varInnovMag[3]; // innovation variance output for magnetometer measurements + float varInnovLOS[2]; // innovation variance output for optical flow measurements Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output + float innovVtas; // innovation output for true airspeed measurements + float innovRng; ///< Range finder innovation for rnge finder measurements + float innovLOS[2]; // Innovations for optical flow LOS rate measurements + float losData[2]; // Optical flow LOS rate measurements 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) 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 +226,15 @@ 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 fuseOptData; // true when optical flow 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 + bool useOpticalFlow; // true when optical flow data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -172,6 +254,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 +278,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 +292,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); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7857a04693..8715ba49a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -210,6 +210,7 @@ private: float abias_pnoise; float mage_pnoise; float magb_pnoise; + float eas_noise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,6 +230,7 @@ private: param_t abias_pnoise; param_t mage_pnoise; param_t magb_pnoise; + param_t eas_noise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -335,6 +337,7 @@ FixedwingEstimator::FixedwingEstimator() : _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(); @@ -410,6 +413,25 @@ FixedwingEstimator::parameters_update() 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; } @@ -473,6 +495,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 */ @@ -914,7 +937,7 @@ 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(); 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 index 9d01a095c8..cfcd99858e 100644 --- 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 @@ -116,7 +116,19 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); /** - * Velocity noise in north-east (horizontal) direction. + * 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 * From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 035/320] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && 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; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_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); From 7cad27a0243a806aa374ffda4ef9e99a854e1c16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 036/320] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..8f39a6330a 100644 --- a/src/modules/ekf_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 #include #include +#include #include #include #include @@ -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 */ @@ -476,6 +478,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); @@ -864,20 +867,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_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; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 9c5dbeef3a8c9024d710e95dcce3d877ba357656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:39:43 +0200 Subject: [PATCH 037/320] Proper zero init of the filter --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ebe30cae0e..94fb31457d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2711,6 +2711,8 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + correctedDelAng.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { From b37d0f8f2e3cb0962155113b07a01830c189d4ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:41:34 +0200 Subject: [PATCH 038/320] Safety checks, prepared to use GPS variance --- .../fw_att_pos_estimator_main.cpp | 47 +++++++++++++++---- 1 file changed, 38 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..9d00f1d17b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -319,7 +319,7 @@ 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"); @@ -461,6 +461,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(); @@ -600,17 +601,23 @@ 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; + warnx("TS fail"); + } // 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; @@ -643,16 +650,22 @@ FixedwingEstimator::task_main() 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; + warnx("TS fail"); + } // 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]; @@ -725,6 +738,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; } @@ -851,6 +879,7 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + } From 906abbcbb6215e9ae30c51efe3b813a71a963615 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 21:04:05 +0200 Subject: [PATCH 039/320] mavlink: Only write to TX buf if space is available. This is working around a NuttX issue where overflowing the TX buf leads to being unable to send any further data --- src/modules/mavlink/mavlink_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a9f5f4de76..2d71bdce60 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"); } } From 1e202a228437c1fa8a04185a5476dcb3d4308759 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:18:13 +0200 Subject: [PATCH 040/320] Updated estimator, not using optical flow for now until proven on the bench --- .../ekf_att_pos_estimator/estimator.cpp | 334 ++---------------- src/modules/ekf_att_pos_estimator/estimator.h | 23 +- 2 files changed, 26 insertions(+), 331 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 94fb31457d..2a7dab890b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -7,13 +7,11 @@ float Vector3f::length(void) const return sqrt(x*x + y*y + z*z); } -Vector3f Vector3f::zero(void) const +void Vector3f::zero(void) { - Vector3f ret = *this; - ret.x = 0.0; - ret.y = 0.0; - ret.z = 0.0; - return ret; + x = 0.0f; + y = 0.0f; + z = 0.0f; } Mat3f::Mat3f() { @@ -69,25 +67,6 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } -// overload * operator to provide a matrix product -Mat3f operator*( Mat3f matIn1, Mat3f matIn2) -{ - Mat3f matOut; - matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; - matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; - matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; - - matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; - matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; - matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; - - matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; - matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; - matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; - - return matOut; -} - // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -1685,6 +1664,19 @@ void AttPosEKF::FuseAirspeed() 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 0.866f) - { - // Sequential fusion of XY components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseOptData) - { - // Copy required states to local variable names - q0 = statesAtLosMeasTime[0]; - q1 = statesAtLosMeasTime[1]; - q2 = statesAtLosMeasTime[2]; - q3 = statesAtLosMeasTime[3]; - vn = statesAtLosMeasTime[4]; - ve = statesAtLosMeasTime[5]; - vd = statesAtLosMeasTime[6]; - pd = statesAtLosMeasTime[9]; - ptd = statesAtLosMeasTime[2]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - DCM.x.x = q0 + q1 - q2 - q3; - DCM.y.y = q0 - q1 + q2 - q3; - DCM.z.z = q0 - q1 - q2 + q3; - DCM.x.y = 2*(q12 - q03); - DCM.x.z = 2*(q13 + q02); - DCM.y.x = 2*(q12 + q03); - DCM.y.z = 2*(q23 - q01); - DCM.z.x = 2*(q13 - q02); - DCM.z.y = 2*(q23 + q01); - // calculate transformation from NED to sensor axes - Tns = DCM*Tsb; - Tns = Tns.transpose(); - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ((ptd - pd)/Tns.z.z); - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; - // divide by range to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - // scale optical flow observation error with total angular rate - R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - SH_LOS[3] = 1/(pd - ptd); - SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; - SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; - SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; - SH_LOS[7] = 1/sq(pd - ptd); - SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; - SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; - SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; - SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; - SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); - H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); - H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - - // Calculate Kalman gain - SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); - SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); - SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); - SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); - SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - SK_LOS[16] = SH_LOS[0]; - - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - varInnovLOS[0] = 1.0f/SK_LOS[0]; - innovLOS[0] = losPred[0] - losData[0]; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseOptData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); - H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); - H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - - // Calculate Kalman gains - Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - varInnovLOS[1] = 1.0f/SK_LOS[1]; - innovLOS[1] = losPred[1] - losData[1]; - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) - { - for (uint8_t j = 0; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j <= 8; j++) - { - KH[i][j] = 0.0f; - } - KH[i][9] = Kfusion[i] * H_LOS[9]; - for (uint8_t j = 10; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - KH[i][22] = Kfusion[i] * H_LOS[22]; - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; - KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; - } - } - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - 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 Date: Mon, 21 Apr 2014 01:18:34 +0200 Subject: [PATCH 041/320] ekf_att_pos_estimator: Using right app name --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0dafc4311e..59ce45e16a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1211,7 +1211,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, From c08544721a0ac4293786ce410dcd0084e1f7cfe6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:20:35 +0200 Subject: [PATCH 042/320] att_pos_estimator_ekf: Update filter to new filter API --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 59ce45e16a..8287757196 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -978,8 +978,8 @@ FixedwingEstimator::task_main() // or the time limit will be exceeded at the next IMU update 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; } From aa3aafb1e5d060593529af72bcf22d6351374df9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 10:47:15 +0200 Subject: [PATCH 043/320] Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset --- .../ekf_att_pos_estimator/estimator.cpp | 139 +++++++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 19 ++- 2 files changed, 123 insertions(+), 35 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 2a7dab890b..18e17d4d9f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,7 +1,37 @@ #include "estimator.h" -//#include #include +// 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 + +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); @@ -15,6 +45,10 @@ void Vector3f::zero(void) } Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { x.x = 1.0f; x.y = 0.0f; x.z = 0.0f; @@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() : storeIndex(0) { InitialiseParameters(); + ZeroVariables(); } AttPosEKF::~AttPosEKF() @@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED() void AttPosEKF::FuseMagnetometer() { - static uint8_t obsIndex; - static float MagPred[3]; - static float SH_MAG[9]; - 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 magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - float R_MAG = sq(magMeasurementSigma); - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; + + 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]; @@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer() // 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; + 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); @@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) 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", summedDelAng.x, summedDelAng.y, summedDelAng.z); + err = true; + } // 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", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + err = true; + } // 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", summedDelVel.x, summedDelVel.y, summedDelVel.z); + err = true; + } // 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; + ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; + ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; + ekf_debug("P NaN"); err = true; } // covariance matrix } @@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); err = true; } // Kalman gains if (!isfinite(states[i])) { err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; } // state matrix } @@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED); @@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) 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.q3 = 1.0f; + magstate.magN = 0.4f; + magstate.magE = 0.0f; + magstate.magD = 0.3f; + magstate.magXbias = 0.0f; + magstate.magYbias = 0.0f; + magstate.magZbias = 0.0f; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM.identity(); // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables() } correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables() statetimeStamp[i] = 0; } + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 3a8f45d9af..734ad0c05d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -33,6 +33,7 @@ public: Mat3f(); + void identity(); Mat3f transpose(void) const; }; @@ -140,7 +141,23 @@ public: 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 From 595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 11:02:27 +0200 Subject: [PATCH 044/320] ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence --- .../ekf_att_pos_estimator/estimator.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 18e17d4d9f..95afbc181d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) 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.q3 = 1.0f; - magstate.magN = 0.4f; - magstate.magE = 0.0f; - magstate.magD = 0.3f; - magstate.magXbias = 0.0f; - magstate.magYbias = 0.0f; - magstate.magZbias = 0.0f; + 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.identity(); + magstate.DCM = DCM; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions From 8ae50a4ba524f840dc7d96b79c5dbad1b7be15f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 045/320] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..e9c036e858 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -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 */ @@ -410,6 +412,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); @@ -797,20 +800,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_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; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 046/320] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && 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; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_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); From 706d08055d2bf63d16c72e86cd60f0e33a20bc4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:18:30 +0200 Subject: [PATCH 047/320] Better / cleaner initialization of the attitude estimator --- .../ekf_att_pos_estimator/estimator.cpp | 24 ++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 34 ++++++++++++------- 3 files changed, 38 insertions(+), 22 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 95afbc181d..5f4bf07293 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -4,7 +4,7 @@ // 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 +#define EKF_DEBUG #ifdef EKF_DEBUG #include @@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KH NaN"); + } + if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KHP NaN"); + } + if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; - ekf_debug("P NaN"); err = true; } // covariance matrix + if (err) { + ekf_debug("P NaN"); + } } if (!isfinite(Kfusion[i])) { @@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) { //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 734ad0c05d..5e60e506f6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -308,7 +308,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/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8287757196..c8005f3d3f 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -606,7 +606,6 @@ FixedwingEstimator::task_main() /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } @@ -650,14 +649,14 @@ 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 (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } + last_run = _sensor_combined.timestamp; + // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; @@ -823,6 +822,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -830,27 +831,34 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + const char* str = "switching to dynamic state"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -912,7 +920,7 @@ FixedwingEstimator::task_main() double lon = home.lon; float alt = home.alt; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = home.lat * 1e7; @@ -942,7 +950,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); } } From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: [PATCH 048/320] commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f2..077184bfbd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) 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) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; 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); From 904ada124baea8ef744535053a0c3b40871565e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:15:33 +0200 Subject: [PATCH 049/320] ekf: Put reset statements after variable zero operation to ensure values get initialized correctly --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5f4bf07293..99de161aac 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; @@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) states[21] = magBias.z; // Magnetic Field Bias Z states[22] = 0.0f; // terrain height + ResetVelocity(); + ResetPosition(); + ResetHeight(); + statesInitialised = true; // initialise the covariance matrix From 39a0d4e54db1252678eb8f4ebc872c589da2d9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:24:29 +0200 Subject: [PATCH 050/320] Better error handling / reporting in filter --- .../ekf_att_pos_estimator/estimator.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 99de161aac..14761831cf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2243,18 +2243,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, 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", correctedDelAng.x, correctedDelAng.y, 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", summedDelVel.x, summedDelVel.y, summedDelVel.z); err = true; + goto out; } // delta velocities // check all states and covariance matrices @@ -2264,28 +2267,24 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KH NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KHP NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; err = true; - } // covariance matrix - if (err) { ekf_debug("P NaN"); - } + } // covariance matrix } if (!isfinite(Kfusion[i])) { @@ -2293,6 +2292,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; + goto out; } // Kalman gains if (!isfinite(states[i])) { @@ -2300,9 +2300,11 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { 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); } From 125f0b2f88c390bfde92ebe5423a0913e0e1b114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:25:38 +0200 Subject: [PATCH 051/320] Added trap to filter to catch NaN handling --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8005f3d3f..ac34af48a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -858,6 +858,11 @@ FixedwingEstimator::task_main() } } + // XXX trap for testing + if (check == 1 || check == 2) { + errx(1, "NUMERIC ERROR IN FILTER"); + } + // If non-zero, we got a filter reset if (check) { From 5b7209639d42290dc5b1e3fc53fc81a455d75b51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 09:56:38 +0200 Subject: [PATCH 052/320] Revert "HIL: Increased MAVLink link wait time based on previous experience that this is timing sensitive." This reverts commit 78bf7ed969624eacea35ae2bf402596aecb3c5a6. --- ROMFS/px4fmu_common/init.d/rcS | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ccac0a2631..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -403,8 +403,7 @@ then then if [ $HIL == yes ] then - # Sleep required to ensure the link is up - sleep 5 + sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From bd637697e4880d3efbb79c2c05647564d3582cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:26 +0200 Subject: [PATCH 053/320] Removed verbose print --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ac34af48a4..ad7cb3687a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -920,6 +920,7 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); + warnx("HOME SET"); double lat = home.lat; double lon = home.lon; From 1e80e624916a0eb1b13adccb4f700adeeee66bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:44 +0200 Subject: [PATCH 054/320] ekf: Better variable zeroing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..ac9abf5ca3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,8 +153,16 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), - storeIndex(0) + 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(); } @@ -1967,7 +1975,7 @@ void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, flo void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { staticMode = !(GPSstatus > GPS_FIX_2D); } @@ -2515,6 +2523,7 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + } void AttPosEKF::GetFilterState(struct ekf_status_report *state) From 4585df1182083c39f2439bb7b88953dcc3575240 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:31 +0200 Subject: [PATCH 055/320] Robustified filter init / sequencing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 6 ++++-- src/modules/ekf_att_pos_estimator/estimator.h | 1 + .../fw_att_pos_estimator_main.cpp | 13 +++++++++---- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ac9abf5ca3..5de22fdaec 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), + refSet(false), storeIndex(0), gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f), + VtasMeas(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } } @@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; + refSet = true; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 5e60e506f6..e118ae5734 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -211,6 +211,7 @@ public: 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 unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad7cb3687a..19111a9b42 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -192,6 +192,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -720,6 +721,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(); @@ -859,7 +863,7 @@ FixedwingEstimator::task_main() } // XXX trap for testing - if (check == 1 || check == 2) { + if (check == 1) { errx(1, "NUMERIC ERROR IN FILTER"); } @@ -907,7 +911,7 @@ 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) { bool home_set; orb_check(_home_sub, &home_set); @@ -920,7 +924,6 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); - warnx("HOME SET"); double lat = home.lat; double lon = home.lon; @@ -942,7 +945,9 @@ FixedwingEstimator::task_main() // 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); + 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; From a30411e9f2438018a08c0965261067940f88be10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:53 +0200 Subject: [PATCH 056/320] Fixed printing in attitude control --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 2f84dc963b..5ded7aa76f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -771,7 +771,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 +780,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); } /* From 7e621070ca0f002e2e1ccd863c31a24166ece0c2 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 18:23:27 -0700 Subject: [PATCH 057/320] renamed mission_switch to loiter_switch and assisted_switch to easy_switch --- src/modules/commander/commander.cpp | 10 ++++---- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 24 +++++++++---------- .../uORB/topics/manual_control_setpoint.h | 4 ++-- src/modules/uORB/topics/rc_channels.h | 4 ++-- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fac..ee818d0f5f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + 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.mission_switch != SWITCH_POS_NONE) { + } 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(); @@ -1296,7 +1296,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 == SWITCH_POS_ON) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { + if (sp_man->easy_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) { @@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { + if (sp_man->easy_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c04e176a12..a34f819235 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); /** * Mission switch channel mapping. @@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 04b74a6f57..469fc5ca04 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,8 +253,8 @@ private: int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_easy_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,8 +296,8 @@ 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_easy_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -507,8 +507,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* 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_easy_sw = param_find("RC_MAP_EASY_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"); @@ -650,11 +650,11 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -681,8 +681,8 @@ 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[EASY] = _parameters.rc_map_easy_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1415,8 +1415,8 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.easy_switch = get_rc_switch_position(EASY); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b29..b6257e22a6 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,8 +78,8 @@ struct manual_control_setpoint_s { 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 mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t easy_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/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd6..d99203ff65 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, - MISSION = 7, + EASY = 6, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, From 831a7c4a833c68b1d418344e2f3aae2c80894b1a Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 20:53:07 -0700 Subject: [PATCH 058/320] Added RC_MAP_FAILSAFE parameter for customizing failsafe channel. Default to THROTTLE --- src/modules/commander/commander.cpp | 2 +- src/modules/sensors/sensors.cpp | 51 +++++++++++++++++++++-------- 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee818d0f5f..43a4ed8ab1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1230,7 +1230,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { - /* MISSION switch */ + /* LOITER switch */ if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 469fc5ca04..8b9aee7951 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -175,7 +175,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func); /** * Gather and publish RC input data. @@ -250,6 +251,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -293,6 +295,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -499,6 +502,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -642,6 +646,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -1275,7 +1283,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; @@ -1294,6 +1302,23 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) } } +switch_pos_t +Sensors::get_rc_sw2pos_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 { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1318,13 +1343,13 @@ Sensors::rc_poll() /* 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 */ + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } } @@ -1414,10 +1439,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_switch_position(MODE); - manual.easy_switch = get_rc_switch_position(EASY); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE); + manual.easy_switch = get_rc_sw2pos_position(EASY); + manual.loiter_switch = get_rc_sw2pos_position(LOITER); + manual.return_switch = get_rc_sw2pos_position(RETURN); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { From 81c03154b96cd3a087873de1583356df5fb4dc88 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 21:49:29 -0700 Subject: [PATCH 059/320] Added ASSISTED, AUTO, EASY, RETURN, & LOITER programmable thresholds to enable various user mode switch configs (orig., 2x3, 1x6, etc). --- src/modules/sensors/sensor_params.c | 80 ++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 81 +++++++++++++++++++++-------- 2 files changed, 138 insertions(+), 23 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a34f819235..48e5d80e73 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -684,3 +684,83 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); + +/** + * Threshold for selecting assisted mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channel= 0) { - float value = _rc.chan[_rc.function[func]].scaled; - if (value > STICK_ON_OFF_LIMIT) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; - } else if (value < -STICK_ON_OFF_LIMIT) { - return SWITCH_POS_OFF; + } else if (mid_inv ? value < mid_th : value > mid_th) { + return SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_MIDDLE; + return SWITCH_POS_OFF; } } else { @@ -1303,11 +1338,11 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) } switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { - float value = _rc.chan[_rc.function[func]].scaled; - if (value > STICK_ON_OFF_LIMIT) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; } else { @@ -1345,10 +1380,10 @@ Sensors::rc_poll() /* check failsafe */ int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; - if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } @@ -1439,10 +1474,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE); - manual.easy_switch = get_rc_sw2pos_position(EASY); - manual.loiter_switch = get_rc_sw2pos_position(LOITER); - manual.return_switch = get_rc_sw2pos_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); + manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); + manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { From 971e8fc4ffc6fc50ffaf257c473dfa86f5dc2d11 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 23:19:04 -0700 Subject: [PATCH 060/320] Made failsafe more intuitive. Default (0) maps to whatever channel is throttle. If a non-zero value is entered, a direct channel map is used so use --- src/modules/sensors/sensor_params.c | 23 ++++------------------- src/modules/sensors/sensors.cpp | 5 ++++- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 48e5d80e73..a19180ad4b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -538,28 +538,13 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); /** * Failsafe channel mapping. * - * The RC mapping index indicates which rc function - * should be used for detecting the failsafe condition + * The RC mapping index indicates which channel is used for failsafe + * If 0, whichever channel is mapped to throttle is used + * otherwise the value indicates the specific rc channel to use * * @min 0 - * @max 14 + * @max 18 * - * mapping (from rc_channels.h) - * THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, * */ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index caf0ff6fe0..28c08422e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1379,7 +1379,10 @@ Sensors::rc_poll() signal_lost = false; /* check failsafe */ - int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || From 85cfeca194aeb834e0d3ede0bd5894c77a65dac6 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sat, 26 Apr 2014 04:40:03 -0700 Subject: [PATCH 061/320] updated flight modes diagram --- Documentation/rc_mode_switch.odg | Bin 22232 -> 19757 bytes Documentation/rc_mode_switch.pdf | Bin 28728 -> 30076 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 29d738c39c9598003b84fe5bd1565cfdff644ee0..70dc503c18eb76c1a4ca5feeb44149e8118b980d 100644 GIT binary patch literal 19757 zcmbTe1xzN-y6*eo?(QxF4DRl3gS)%Khr7GGySuwP4DK!igS*QBC-Yxx-|Ty{&dxoj zf2pL?$@A9JRh{%((yb^1296E@Km!25+L@wxVD!VZ007``fd&8owwAW0&K?e?h7Jxk zmd1w8miBfGu68E$_J%-9AiceVshx?vv5T##oin|;lc9r!r7=+P{}mAM-wtd503c#- zXJ%>c;`C1_ATxuRjlH3>sT0G$RM61S|Je9%A^_EYP#GE|Lbo4lkz{cfvu^X%m1vx{ZIATJGeOfmHrPt|C0{rZ0PLrf8o;E-rnZ_+~r@D zgQ1zJw0fSZ%0065~N>oV2J^M08&iX*QbBt)nJ9HC14coK61v%FNOLM)$ zWS({%&xHXV0SN7eTf8h38Xt38N@9YBsF9rlzc}|05=z(E(g49y=~jlCcaH-UVk ziEq{$Y^QoUawlWMbsJ^ECvXz0Qb{x6`f+~wWBDFx)dfVTagyli0|4Eqpa_sG6#^m7 zfQ0Zj!9@ooTK@mLFwqVWw<@t{(UDtw4kGp7+_k0VGd%B?q>XBjb}3)j)!=okn_DQwAkdy81=re!!9|szx35E7Ed%ubhBz9e1+M z-O}5B3W-;qIp9O*2j#!cnqddt9KK}gP<$o5K!zt-04)arU;Xi_+A8f6y1CNf!z7QO z#34IpvH3?D4nIYDto?<$V~O>*%FoR%R0#yL@q+hXqAu4(1n%5rZmXXaLgF9eX^_*H0gg}n-%KyyHSPonaGn# zTa=N8>ttwLLALhp;*~S@PlE>H?+(1g!$;zzZ#G$wS}Q;KkrJ6f=q}z?InTFB+1?r5 z^z2uc69g5&<(6o9^=TIL_`l^)jBnpvzAf7p?OMFJy!?dr!q42Z(n3Dc|KXiOt+l2M zGs03rDnKTdaWy};$C#NHadb=}YR@#0$12tIL280wiP}F1Ql${7thVvW=wb-dcdBJ0ch&^~h^SW;~ z6mvcSvP)VVG$&Tz>}Cx9G8HuzmRlL6)|uUx`f+aM1Kko;u=y(qz=X~@Q{Z!}tu#zw z`KC1A+c0sG&B~p480kG?$K}!mC)S7k76jF$K@kLM5yd);Vat$tV6H^tsGKlc_qOv5 zXY1!%AybeWjA&S!{Yk!X;)cy1#%2j(8^d4c$dX9_v$Zaes7HFt$B}@ijgMT+^VH@C z9mh4-z#ZAS?Pznp6 z_BF01)d@g0sUp_`Xnc@uQqF@4#@i<8fEYZ+UHBiu0)$)zg(3cm@K9+0;Ohud=zm2s zl7#?d6LWI;zoIU3M^M3d-9S)5X1PXi{|_RQF>)CIbfYmUIiUJ8VLOQ!gn0Z)DiTot zCh;V!1ny7lMS=p_e@3*96@d&0yomaM1628$#9inEpc^Hg=>Cd60>1+iEuX#tsz1}} zg`SCCC>TJr{SSY2@}0r997lFuD^(m@34Y+Ob}oe_63Io1#&7(gcnrMg0G5ydaz1JDP;SC1VKCr(4m!$#lFFXh3G(Pe^Udv zcpY%LWFXg>zw8;S6N1t*Pd5VWz3T;+Xd|u|Dq+Ojfx2m=Y!eZP!J~k}+9^^Wj!3Qi z@{Y(yXtMp$<=S%y@6cRpfl$w}?I4_!ct0kwrs#P{{)-#;j!@s??b}DO+pdKuY6uGM zPJjM28$5a2G?y5^$`di5`qTX8?DO>+ysw1dMizwEE76Wj;B0EU$b;UyJ|j!~fX*-1 zc20WIa<)P6QlYWSAOqYg@VhGxvmSkd>L9eqMw*K z7?3ODRRPMr8+s?Wu)aDNLJW9wCxO6{jdhk-2U_*DU7rgWpFP6(*l&TCL#X}j*F(7T zx$w0ZqU=TY`8FM;TIO6>SK2d zAfd>ew1@YH%GvMmRoxiFA8|a~mwf>`TE$P8F`L%ph}d(^?!NGd=y^~mpTihi79BRt zcGH6y_!JCx{u#D;?^u-rDp63K<)SGx3GPJtSt9r-&t!v!k+^21>nSabHX=?JKt#q zi|92lvyiJ*ypzfaGgz0*DUqpW4;9cG<>!9Qws-qK$>g8A4I$cII2V0`7I^d&++Y)V z7Gb^m5oR{sd$j`KYo*FF1=V1-D(@2JJhRw%5wB?o<-G8~+M;<=MmN~n^1Q@5Cr6+CjH(s-td zZ|M@;se9?B;kBgLt2l<)ESEzFlsb*WKfopy;swIrB<|Xi8T*%^w_hcnD0hn<|Fp-Q z_TWgWr*5SvUPi?~mhgE+>gy?NQi9CNWSB{2B5{XV$@(2TLI}R+!dR~r$c01qrB$9` zd{$yH3#kOb48J-7ko50oPSnJ&n%xPyGwU$6B(C}1PlA#%z>F}}i_X^&fG#FODa4JH z0Xa@8);qNmB<%_ZSc9fBdTc{UXC6!|Bx6T*A{QBr5(u#7s;yT;3;!;-5#EhXupvmz ztG{)iOaV65+gYjhe%_7P%K>6QR!=`ToD2eFN8a5^aH;06L3v9Bi+2Vg9v?+%&Rwby zX1D?Xo@fC!;Hfu*SmKQleUVzJ6F1cJK?dZA8f?{3#ElcIUwm~M7n*=IXyPmrt88>Q z8<6;+fi{d%BTQ;=JP?ziJv!t>)ip2DMtlJwQgHtmKqJZm2ef#+T;h%pBRlB042)iy z+&VF~5c|;EvH-dC-$)GP1aL;kCNi9AelErT8>WwS~ZxmMKT{pzI&|Ap(6W{Qb&OH zixW#Nd7R)PsvA*7wUUOC>@L6pLVk8$&ehOz>aKWvIw8%{grhoh+rXLd95E6{fC{3D z#Wc-aYW2v*iL{v}T!y`3N;E4z!q?BWR7gKWBD`x9@iv5v=%QyR{_P0kBi#oN6(4hx zDW3IW5h8brAo*cUobAN0Pg9}uO%o15npH4jsl5tZsuY(w!)mC0aRYPw(a>cwW@dc> zRVN7l3x5hknk0cBE)k(0qU*mlhrVGoR!KGk>QUBce)es$`d*6pYP6i$!pxz1L)&yC zBK~|ZeDFB>Mosx4Su{`Wnpg}++!O-f5#5b1N~-EmM!L!D5Z*DR2b5ZxmlUNStlnC%o;4)G$rzeisY=e-Fzq(-EE`tOcz zjRkl;WW7DY^($n;dh;tpdVTFDB)ec1^%JOao>eo?W~=T1>+XRwH2cW3pYcQ9I9K(w zH5w~_AmpmQ$C^+?T?BjBvnUnKQ{2RxCir%Q?w5Ld>x53K-I3d^woiwy zQ41hDm*2ZXlj6~#iQz=M-tQrr3lTJcH@Xr$<3UN?iwy1+!%Aq83XX8p1;^$vU zBF`$8CIUg?BJg)#kbRP_g6XWCj=jm29GQZ1Ul+AFs9 zVqAld#m+!%%e8!P&;BkuHr&B@IQP1} zNXCaXzuv=3-s{csn~pIzCrMI%D5A4RAe~WPZzU_q9Xda5BRGnYOeDXap4&5snRL6a zuR)0l z*wmXwrN7s0gy4AEGfH@gV`b0umk<^n|D#BYX-d$l1RVkaCJ zuY1+(of;JV(Q>%d5qzo*169igMB0GB!EM=8>#2sjOF$hMnk6y~Y|WxThwrrSaP0yo zWtKfwCRiKJsBLKHL{6~}tdmlTqys+;Vs_h8zrfm0X+YdmEfh6%BxbE(;n)Dmhk#xU z^7ovinYX~$IUsS#u7KnkHZf*(_@XdI^`FGzH=FDN+tmyrdt(A9UF~0tD`0enP!JYP zP4RGL#OCxAsge-Px+(Q*;3!40A4Rnw;4zRI$_p7qFFO{YZ$L zJE;fq(iIgf2wYz5dZDeaa{*)ai2DYBm28@9?3gt#ywn$pW1(_L9eOb>)O2%BZr{A z-r+WfsBBnIz>F58y~i`$1CrYR0<}srz57ZZLGL-wYt@5O6UN|TS!9Yu5QI2c^*cZp zLL6tKsQ(qV;xfZVQosCT{XrdcuF@}#&}an;!k*C%A?B?a40U0h%pc3f-9ly53j>P5t-;(ZCJk@_f39C;Y!zRg z)gQ0T`D(tw%h{7veqI~911SeM++9@sy&ffU{n*q%&=b}8Bj!EBo3RuB)X{WU>4WA| zO4tu_tsTs<2!?*COEnwJ4!J5E+HCiR7!nBfRl2@8r!`u!trdiJup>F$uro#-Dbvf8 z5HH8~{ejmF1It1;-EY+G-G}{T5hcuDI*;J(DEfYXtPnrLKnaxP4?#7tJ6Q(qFioH8WjvdF@^S^a1}rdt#Jp4^3vv>O)7+UK5>|aR zspjGWc%_7x%)^KRk47iRD*lfReya)e-=rn4RMDN#G|~H`#)}gX5u* za}6^GcVykGLT72)g%LBDw~*MBKH%L+wE(wV`87E5EiqhSi7RR$D>K!T$ry^a%;X!b zsh`nm4M1;I$d}ve@UC#VgOKBLU}|Ze;3UKO1x!7=|8L&117NY%{m) z5}Wv=i!=RhO7b=t`&;Lyx)E|YAl=*fh}yI~f;>$xf^M@ zRtjRhkHOH2-F$rWpdY(q<+@W*TrHqR?-GT{KX1!H)Ml=7VArNQ=OeejC;ir^%#m^? zCX|mDwx&yLbx*n5Uq2u2T|S$JpPA7%zydY!f%;l;?aqUQ=@L;g^sL9g$a^{^mHIgI zE0gWLQ#&X_i@t3^{}tJ)(06F6VdoT9yAG;lb%SG(WKmrI$z2aFzaMznRf39zukp{6 zN%5UG(}css;xp$YAV((!{Wl}Vm?KNvFx}jMG0#Ik?ZeKHhT0*{;kXD%7DX)y-32tv zhQY@E+|;QvcmbC&{v^cbWLeS_Cle`KErn8?9)xL2O&VCzSwympRo!DJ3|f)O@X61e zhN5R9b9vtP+J2=5r_;;Oxl{Kjuz~y7wQG(_@_veMe<*%jktVs+N5>P&t`~fa8E{~f z+E^yVRltD0V&LO*+6G+W4(3kUV!%x`D4_PzH7w}yY&XSO?mjBn+Ak5_GdXM+$o%sd zSwM?0BftVCSS?`0ndNw8ZvOSY7JB{Vb$)C6xas)%Qt;)V)B9r3JBvrzzrDG4hGp>y z?mO=I{w`q$KXRLPWtoJRmAm@rLFV6f@v|v-gc+%1h?LzN5v)0!A{IM!FTCwR=@Xi8 zVmE97s)c_B`0B{Y+>;6Q@utVigE6x+z_Aw_yZ33$@s)|kP#-?sPiJToP>6sF)#y8- zpScxdPGwX@^sc1+ONe|Vy3NJsCni(RWGNTf5u<-QX2aF@oVhX-!H^a@qtC`E;5Gdx zdvibr{CA@)sweC{S(hwg{jB-=UoZAtzXR0%>@PR5Twc^N^M^>75wm1zef|hDv-RHQ zd;D%B`lBhvU()Xv{qSpO{RviLU!jV-%Q7 zRr_Q6oYzV=QELbizY7NOEl@TQ9U=A}_0|f<0?z9K$iMhId|Q0cbxN?Nq{(r&6#|3h z=!`m&G-c{nLBhe6@^%L|bgNimr&253@H0 zrRLcv|2d^MMPsty0qpEM9!@m%EG;ouS25vAA%Ar5O|*7TlfGou&SmRyD|0LQ*jSp& zPeotB2m620=9Ff@*e*z^>_8%SzL#VcvC5Qz?i$%DZ%f^^HL@-iZ@MN%g*s3@Q8Z3k zkx>3u#1!U{sI6+ias|F7JH$R@z602l%Aqc<*2msiHWMuuI~!~HWzr+K;yf>~|6RzS z%%cu`UkGmYLQk25nAQC3E9q1o zfjk+tBee*I{>p-qiGm{7iI#K7q5s2#_^Y4cWz~T_HCW%@IejTHdC^*7gTVj&QV<*f z05G<M*xerBg@11k@Mr(>esG^q3V9>fi)%2P$Z?ph zC*yL4K_ahAAf{!0vr6D&PczjvF)G8H9&0#ewDef~A*rZi`-W7i2md?R^B{{bPq#x1 z1PlY|{os8>*6&VgY5n0@>Du7=$Y*$?ZT$uDnxe%kut5dS6 zxGm}1cb#Uwh&c_ve^fDelQq(6@$$63zyCfViTo2aad>(P!xu3QcZ61b{zfi8jlmA& z^26b5SenX44yz9fL5sd2PtuJaKdB^2`d)cRkM&G`VuMS@Y}IlGMP5wW++J>YsP5bNg2g_J1dft_DC0dUPR=>DoHyN`^(L%$xeNs@VqJA zWtobl9@b*PfdN#!!q#?y`+FGh9B%jI5>nOu&UjX)6CA%zG1>b z1yP}VIGwkVGS7|FEWDm1h*2sLxbLvyey(qCe>DZw$hb1-+I6$$IyCOEnhJx}y4ZnP zZX;5}rqcyfHfU_AMXMj3vvBl4Mc#!g><+BHA{WdfJ!JO`$CPIYZdj6aCY&{&_t1zCYqKwr%U?^l>ReVhf3W4es!WhS+I(6DHx_y5c3FdC}3pn9c zI{e$>=-`Lv0J8<0_Ph*CSd5Rcym-=4^?iP-3nBJrkgZvo51GMG_{$jQ#PG(~6tr2< zMXzOQgF=*C;5a3T#C!%n{f#fx0hQONVuD1s3BC9T`XZ2bl5h9+whv{jxfC1sk38s` zVC2v9?!du2Ob82+?R1-UL|d2I;ou<`mY-e)fj}a+(q1%Rpw|LYgG(y zlMu#LBe<;A!wc7Hz0v!AFI)j+uTMdq98*pbxfWilY8_t zwr|rUYUpWVHRRwE#tZ0LSxLe!c2(Mj&z+A~Ut&`y6S0Kdro*zYjnX?UlnfHrpjHdL zY+EXhLshu9jNA0=5=s>4;L$gRD&4GR>huyEtunpPf6{ndv7W?;g!~AEi{fX{qC{pJ z%tL5jo9NzVSr^e~Ib?YXLq>VO((Y|wE%m(_%~V31JlrfO?gk3TMk4Z*vfUN@5^&z1 z#O8W=aR;!3zCh!dpvM(s`^iQ=;Sdgoal<`>ArrAXYm}^0lny|DTGi2;6^v?y2b%Uo zE@;g}UX7aXprSbwZ$x->Js#5=In<(TfS^>5>8Mg73Drm(SjzKp%1qa~kz z;gbJ75_+hrd?&MseFSpVHsE7lJ_dD{meUY`!Vt-W`w9_Tl@uy1UIs{!2%R);-;-~r zA`@HUcr>e);(0p4YHx9M61l#R!KD3gf>L)ncKRw3M{iCSulCVP{UhDLy0jx3anV6( z_*069w=FcsCok(TVsY{prmgS6#Wz1TZ-zhb5_#=gIs2g#vpOj)hwkz^sB4s$vI?%) zspQ@~%$5**xB)qIYo-CZ@;M6l1Eq?O&O8haEd<;y2qkOsi*Ki>V^(UO9S@3;Rd^-o6A$fL5 zNHL$5B##W-gjZ9%e{oZDd?N>3gt;Y=i{BHMa`wuT8Of=vqt2>>T`x>3!w{^D(gqDA-uoH;+ z$I0Zp6zGgk>tW>R4CqBF$2h$iy_NcYc=t;Xu4_d$m?jiDMjBVIp(s8KX*X7k9sD7! zK_s!9xtQdp*O;XrA*3ue6VQZjMK3Ew_^D9U_cG;8FN_zWt&q3yps>0)o98Q!Fgx5R zeY1#)_8dUa&r-y~XfIS~fFY2ZLw;X0RyfX;i&j5a_Ei@05OWfMDM0LOcI4HH2bFEC zw@F%014k~C1j01AHxAyaHQ@0~yjtVXP_`|c8E zC)nzz4}+0dc6nsE%`BZ7OB3-+Ur!6Tofm&aCd1ty#Pvg~R(I{8cD^E| z>2#aIlX&HA4PmR3%k^eC2%G5=j`HlDxn8l+S>s9KC!-11BGzKOSt-wjB%B@l(RjK7=(37mQ*kXYnjzvQ zBh%w=(G~PuCJRKklYabgHl=NlDkL(~05L_(G5TMA&NAE2CmtA=ReO(}s zfPcUSfHfOduZ1@l`9-V7)6+skt0n|4`(o2Z*Y0d@`?P22^+6>={F`M{9cNQ$$+32U zDc@XQ@NkHKAOH+sZKsfsbTD$pY~yt6J(yEl?_D0va{5Om=BKG9J3B_jYDA||$JP{U zC?&?;Ek}|(7AArwdU1yqi|dxCahK#Xd~*?2WujN4Da8`)UnzFQ@=@lO+2sSFCqtR9ix`W&L^9MPpj*<4&a$rvu$z9+w zp8R3snR^dTJ5@@U{L0Fyq={D1z=pa4M6nI4hR0(wtE%FUuaL#fxa&H@eRNFufC>Kz z`P_+(8_ny0S-MWsAr)g0=O_(L-WYYjTY+=q2ZjUgKfE0}b~=s*yov|g|lHatCWgnnLqy}UuaPsD5>^kf?2=ni{52YA*Z6f@hU#8=lEC6DfI*;(-&Lldt zj&$e^zkeSgJ#r+nGoH51F36hx{*s zpSN+KO0nA{^|9b`bf@2n>+uE3LF2FXlbor+Z9>quf__WMsl>UIB|${3#`*dP!9-At);{hL-4YQAv@H-{cvp zXt{I4FZ!dpiM#irsT+$Zw9BHRy*3-Zh?_XFY?p4YQa@zP@Oe~j16za4-zGdT+ZeY> z_8nT!8HyafT;{x={5Q0qnyt%QzR`&(qh{(~5nf6y8v-Y6Pc7I?$jsT@qNBqd>ZonYyDMo6^UFJn zyN`@;sN~U9$QwY&B|MVM7hhKys?=-8kydU)@TAFlTa3J zfBe5dGKTMdRnpK9fY7I<`GnkMR&II8wPfhtS)DtEhlfToY@(oG2IoQ*-WAc#_B=-$ zK#~Lg!6BABKb6edq!?N_0kYYJ&J$G&_;B))8Tv;JL5w^t-)~l>MQr^_g@SgZU9RMs zghZvH$^-tfU*%tb8B`Ktcg4ApCwt3b7xgxQ7&^Pm5>L7w0~-GY7+_uGJ=kIY8L~_< z)0;+*gh0yc7FH8e5%3TFTuOo?yMwM7b}sGSG0%1@F6eD%FjIj6()Un_c>EII>_h4smev*8ee46}9U_oUXn9Y0ZA^Blh z=eG|3I;Z`m-zG2x&#s4E`a6T-CTrpF{eS7EGPpV|hf;z3H#^r3nqnUcnqUTy|L_?a z;6c>zT|@lWTaT=zd(_%u)&QuEx4;ejJqZ3Np!qLgfDMsuO>pCaM(6UVWvqO3)8N1< z2Qpo79VKoU*90$aW#TmmXb@-+=>2RMAqlMzWLENVrvzm1-WSp(y2FwblpO|XHg3JWe-o(?lUW^lXfdQDQuL4RH-W$VMTv&05JivyfLAP# zfA1IlPuo!80RRBd)Y;k6&K&r6uj!?ZF7OxkLa(3Sh>8`1>v;_q6Xg_+lDU}-_MeW3%dhUvGYXSMep7#V!|2zcunEoEmM(>VI=I!d~r9TA9h`P?={e02 zBjHoSJb#~;E95%4n8RvqJYeKbML2)AT1io}Ki}D!-j0F+Cb?PaQq{h6UfD7w+?pY_0M+(2FH89EA^6+GS6AT|P4F z%^sR&oT=;et>zG5`)c={J}FDzeMNfJyXsrDA9e#0h3FAp82c?+KIC|_UOghYyuR$xp8#a$wlBJkEsCqDXW#0{$2d((m`RY&93ZEnj24WXUe(K| z0SD|^nfVk4d{v(i?v7ltP~zYMm*Y0P4{Z36SM)1`>DIJ6^#j^9eCdgq5~uFLAy<1_ z5~$8vVF3@z$C%F3V9>WuKBIPjHG?tbUCBz@pE#txk|MLSkiJ&rzkRWGv%{|pFpqW( z5p(MoxBCyneT^P$G_0jQCx`k18KQEE*R*l-G;BVRY$SK;{%qnU?kdy`X-DLkeF|M# z4_3I0u-p%I_QiF9rrc^r_Ql<2Y30pirVQzvHmHgES{+MyisRS{$h+IR2~IM^GreL> z89U{AIJ4*}urN7IO%4RTk35s3?D>W^Zc8Vg@b4&l;}_lMj$RsviL|X!;k@&h`1<}S zP+e9m<^_IDonmCpo}!xjZnOg5SJg3<(uYLlFyYqPc^vBR()uv-`*&ABYnxn9zrM?4 zFBS`vByA|ll-7_NQ8irX%z-ELHCG>wyic$I70@Mv&m%I| zI5)z>;%<`3U4&=v52&7dz67rkX*P^zLyc@{uZQdHC?Jyj{H?2iMjl3G1s`Y7_S28y z)0Kw`L+J$eIa5NhJse|wSjWEE#WoD!W>jG>53E1kF1Gl5bRo*je%kxu7QuV-QyXzG z%(Psdnf>t=mrlfaoCOMZe-~`rUgvr$-(=q5J@;)x9uGO@IzHS=VH9YX;#3k0E22(v zOeKS%4{}_caT%VbNGExmwWea>jrxQa0+Cb*O;R4AY&CyrO&fORVETyEeDvX=RU#Uz zL(E?>Rrz>m5?#CH^o4yDXneLqR^Y|J6~tkZyYyhBD?HKJnkoNqF96fG^++l{-4YAfv=ZKEG@t0=RU_hLtrp7nEGmx%JAR>mm+ z@ndgxiziw1Vm7%i)2!mpanE_(E9I5mO7}bEneJM(UeKHB`YI-RulnHQsj=$=h2*kP`c$O zJZthY{K%5HOWtmCwv1T9c|S_$#%GhZw`snZM<6w>0EZv4D`t&fP_8$4kRwN-DwMIV zo+#A;{gmp&Lz#$3o+7da_LI0Ud+LA)AgmUbgrtDh;?+v3OSXcR75yNG;*OO(UGf60 zUEIP!8HhP?L%Uf-wQLwEio*EjpxQWW)N~|{8^T{wwJ=CSaWNWv&n(egFVWILgksBl zHS$#6ARO87AfA3o#YGd7gL9ZLp2gEp8mKcZID6n&26YLU6$af=(pyue*;X#olDOVq zEfdIRg(<0K<^TL@VlzcWaYD@@R-bh0>KRa1Br&cmYX^5YmEVKZs-?>Hi-oygzIOc4 z%F9^JVR)ldfAr1vln)@ls1Lt24j}MciW*Ohp~%rgu(_W`@iHl|JH9ocdV{%oL@+W4 za!$|;!)LXEmQ|PN7Ro=5r{=Sibfy$>6-7AOrW+ulEt5}0OBkQy5VTT!`w{O!`+Q5k z5IB)M-6TZ(HTg6i2_Fv9W_Oa5AmPHKu|`(E+H}>3rwq?u5N2ye=L5B z;=oPb=Fs$@(&KD(hMK;rj-nc7wdxljOjsMj6(w(ES;QFmR-4%2f9-9{`+n?`e>^E+>S8(8b^S(ThP!VwzI1^g|Mpp6jG{J96s z1PGL8y&gomvS}Dj${6hfCnjAz7fHKn7l(BZ$QnK#Jnf;@-teBpdU?kg*`uMH1}Tm( z0gk)kP<+Ga3QDzzSiRmuTREH2d9>1vZGCG;rLi1IaGfdI3T8{cFPh`zgT4oHUs3{v z3O2~gGD#rf{f@5vsH`O(I3`}3rYTqv*jia49+)?XZ)8WFIEB$}(vOZ=dheVJd@2LE zs8RSTX?XX7jf0&@k!78&wEF>h1ZVtN!nRvktgI02FgErp+Z%~ILAnm!(H7k5(~u&8 zF$PR2vl!OiP=p(fA!Ty3e&_xwlriL_)gA@*RRdf146*i0kfKh!ZqVGG5Ii=>nd&>J zAxUKb;SUz_^c`*EB1Db!-u{GeW}6yD7#q7dkhoWO4?Gb$FJ35oD`EN{uy89NrNvt2 zq~GW?4pvSanQ(CGF|D}PTp^i5!HrFj<;GYdnW{aoj?Noid{YI2-{X%7uc`b^qw|Ek zC_!X(j_Wg+N|SxOtno-Xa5T44NAaC90Q?sL@^n-qHdDxq$TmYv=8K`Pgky>zQP#-! zVCJ2zep7$Sgrb>xiSh2z5nKUZW^=hV8x7d~?iG8lBECBA_!7-qD25-Z9Zu6gJuR~S z$Z@bd>-t=)^JD{^`_VKH%K-KYS@i4_jvk< z7zZT`X3b5vArV2_UXw?JQJUlG8Dq}gsSs;MilMq1p{z8>In<)e9ln!OK^{W%*+R0W z$VrsLX20udZvEe_`6f^qN!ki#glu)7F1+V@4;m+}H|7j_@dq}dAC6n{0lgBCRVCp4 z4}g`!#Px%Uyalq+s5huPZYJW`DqM)FcrrsO<67%78bk|3YX>`!SWVbd+f&$AA+-Lw zSyzk|u2ie)(6!pOmtMRJm!QMkV2F*Q?g~}|_GSk%gO1fJ$$3$mrIr*O!He%JFRsSU z#jor;(T7@jabzYVH=v_F$L|NRdM{Q8PS3a;@y_%+uvI*-nL7riA=>ajA-{5M zxeRCLsfNT==qz82B~bG+ev|gPxQIPyylB!8KHr2UV$9;qwt?*04|B3p@sL@P!{UM^vAc7hUvW=4-hNgEWjx1WY#>F4sFl_uF++iPU9T2DKoeSKQv7``{?AKlm2Uhl9oCuu@;B-%f z%apSM%9}%q?-unbcpj>_2X@1-c?GcQ5g*c9p9QYn#RLZLjc@b}<3bGh8~+^iVs*}i z&^s3hFkSxQ>?5p4`sD2?%sFS@wX#Rp=$r-4pBT_vp4$vDTzDY+{!_tU!Dn^x-SEtF z5w_|z2-o>DT+L$(N6ilHMeQ!F)6d6`Z(e=jEkzh!NRG@YN99FLz947Bho4^NPm3mv zujL%$o!LoxC&F0Vxi3#)_JwC!r_S9WZJE$+C>>n>6wCMGz6zghx^3%S3k$&?J#Kw% zE?-nqRz+j|93fNcD92Z>6ei+}N<se z^zr2X*-oMKTC(7J**)~YzdHB>k^U6Kz^dPzA^Y!YI6d^ zj9!?_0{AxSCqekxHy9+~=@pQiG2l{tZ{ABMyyM+h<@Tmd*hf(q0@A81o z9yX@`jET3kZGoHYsGq)lfkW~_D4t= zB860KEG-;qiJ(c(coq5T7O{%zpY-@b+sqHE;S%;7SW_uXJ3>GZN3SI�&{M(JkZK@3cB z?0$bZqV^`sC|n2}lh|k4cN}XU50eDbIRP6idd9xX+2uM&>VdqFjE@hLDNda=rh>U^F;7G2x^VAG!lDeu zw==5t`YNJ3>d38Dh_af7Yu%FvVXQx=5A22rp>my*B1W}}N(Cb@?nymv(J1B{d3N8Y z=V~7*gxyBxElQN1EuC-J!G73leg-sM``^s_1dJ!5y;LvQOnjM zVfPmwa}_D_`Jl(p1yWuo_hPj|8W_(ZR2ul?8)h;)FxqocwU!yE&g&BGhVEf^4yU!z zinhe$el?^gmcvC!Mv1|R!JlHF+rZNbRa$8X{AG;RK59FIt@L+ zN^07hNg}tZpdx*!ofVA7z3im$m@+8)bd|=^O!4hC`Bjc$^ksov7D??H?M}x}GAE_< z(Of9V>l#a;0mpF4;YVZ&Wu{;xIs~&sQ67~Me$a38)heAaNk+O!Y^)TJ=uw@Ds5YRz)bgEA9xjDbK&g0s8`-fmDSrd90rQcFG3R z6bTUZ9L_BrmdupoLKuC_{(WFXmbk+`BmRO+mT*5lcMW&&mYnr+-phrbwR1y5R7b+nDd zn>*}IUObV4hHKDz z-ik>L9CRyfYol5WV3S7Rmr@4D#Wh+)o_|^er&4w3c-X7eJZVh0ToI}mTr-Dr5oBHP z9a~A40DLeHN`B>ddk3S0AB7y5io*}En``MR%~)^?mbMf9a)O>49-?QjjvOURfp2)r z8MPWhKZI9~BBZn6-2Qc%JL3tpx~5R$p%bi5W*R5>g-31eOz}Vlk+3=jM@qRReom5R z=8-#Ds!6IKW!?mNtvTXsdLbo^?KcRN6xM+D#9bwdLQqa#pfzng{aPqJFot3!`oaYD zGuD5>s@`Dxc-m zfHx}#ef&h}1)o2LKLYx~oHEaLGrKo(O338QS+q<+;^ENC(M=21HBQ457+>nho9Cwq zWHe34`tfw_jkP~sQ-=mWb!~~&_gSo;+niyNOg`&#m@euwbVP?+!~$Ceblw=W;POmW zdGscKtb4xn@uqNh;&xF;R_oO`GXqOODK$tK%bT>2ms0&X+n|TUuxs97gw(^zSk7@* z8Nk+ND;*ZBhb7DQd|v@XNB9l!yEg4UqU~i3v6mdxqqXqRQ%1USaY(qFUg)z2a{~a$ zAH-WDdd!+YLW1xVH!H`i!=sK$i|5(fev1)I*;Y$J7DG?CUULGWQ)nnld{!H`u;XSn zi+RTrr_YZFUFQ;vI0!}eB8Q1ql$MYIX0>X2((B9n5o6wJ%2D3v(Au?^=Q7$cXu?) zV0HnvL+TzEip}L}`^Y@vWc*$P%(ngz{mo6t4~EoDpaFn2rvJ)K{_9r@)PwH(Mc@Fy z-}2A@D@>gY|IXI)3b@0WIzA_(+-P8A*vv?TqbBEbYwsNL5wDzjKlb@Wb-j zo0(Y}oBpp^!49?11?g9)$j!+swt`66DwP)HS%Htgu*yr!O)a)6Nwz{h2g3?kwH4SR zh)6|FW?r_fQbtKhft9{~d3m{Bxv^e;QM$gNrKP1lSXK$DG$k3XuAsCi2dpY3SwA%= z6?D=@vA&_6p*~D4$c4l@3ZxdvRV20`EW;r!UGaqV5b`y z7?|io_%K%&=Vs;*Ya>XlZgPHZL1IZ}QVvR(Cgx@45^D@dE!d~Xu1+sXNy&k`8!1wZ z3=E9*)AdslOA>XWyVJ?UG1HW%A(R`aylvs{ehoON!NR5&L#B?j9gleQ@2TPTp z=(Do{M`ChOD%dZ&DTyVic18w(AYXuk zH^0a(Br`c%AtXOJ8|-$ND8yWN838&_C9}9BGg$!~2lRWdVG(zmmL zrUz&Y!gz2Kz!8gk*|R7E1B0(?h@-BjpPPPeVqRuiYBBh{o3)b<<{ehxX?tJW)lr@B zI@8Iwdc_v?7jg}P>aI-92SU!??sxIJoqKnS&)%CS{_Vd%?dF?v=fc!GPFCdHFs@G$ zUGQz~!F`GO?bm;`zTLLEH#oz}L*@Abt=buSnTLy9UWOWZE-IR!lKGN3;dNGiY! zxbA%HwrzY}eZ0axI-d&TQ{QhiuI-z=e~Xw`_J)8U`POrWecw(ciEY|?0A zkY>a@y$SWqBitsThFB;TlMrVh;WhIgG^|U;9#Mkn!_~e+fCEkp44`rp)Y^mTf_V_RH~ngtv#Pr* zDl;+_q(MMY0RTtzv7M2Njft%@y_utdy}5;vlfplTkdTo790LCTu73_qoDJyRZLFgb zWn(fJ5rc2O&_bV~zX8A+z1!vZ?dwt*Qe9P%SB18HV$BVbKR$4Qv}a5~SWbkH$u zWv}QrxpP=2)8W(D(T7JZ%C#8dH0dm4m9__WTD}g4y|Ldg+$0_c zrJugHQ$jN{zDpOW4OM6xigoXhL@)7(p2ro^2ms5df~L>%b3o!xEsHwdK<{Iad`|5< zX&B@_r12$x3;FtxfoKA=8JKLk z7W3HG72R-iadELbJ-n@rwAsZvh)8Zx>Y_#AlES+n-JK5)ULW&g<>2sd%?<%`)*dTS zJQ5*!zz4+&yUWQ@bT+r?vI~h>qS_ByfxgDB1S%H*k#<-#ea$t6T00$Kk=(1WCkuf# z2^lnL=~h+OP(DBxSvF~fUS9`;!#YBNiICoU1b#R@Ti{8dk8)#}uz33M6+Nn&|1xN( zmAG}R)1b2Uy%BRF*mG)ITTE{iaT$-TvA_a$!IboHYuoU z2+6`?wQU|>^FEC8@2-#Ae6EkLvy~WMX9rcUSvDG+^5QlNZ&mUiBdoR1zqAc^A+#eV zjuh)wDMPh`9ueRN-4204iug^Jv{1lLs7`yZOGCi_Ti{hYw9};W^=Q0gf9=(S0MtW7 z=M{EKn7%c9r^-B*IY?(KE{7jY*Y~I95>LVM`lY zB>uM=xQEg{M!c9Y*A~3|^o|X#C|(k0^?f5b4EV5ELjFZKJo$|Aw)5_>zw|paGYHpUuSmq5DPGYCJq^wS4+wNWp98V`6EeH9V!xh3nTMDapWYumRFge6TVp8cP6Rd}awYP#8^l zF}bH%o#8<~4FdK&*X6)7YK8{$aN(v&l|-fz&k;kQoRGl@%_`0Bb{*Pd+EK*AwjC$8 zxrJwyPIS~ggY)<2KEWZYA2mw2l!vui))zcGC0yUDjlWO1Hl7Lh&T#m}3|-M@Ye(V) zJg{|d3_avW)%MjTdk5_nJ1IO&?q^ltRmEQ%A1v;faL*xpsmEPwpWhuu>C)0Z?e|w; zPcjW_EH@ELdxZ;Gli>$vg%6uPr)nXt4x2V!Q`64txRE6jL(cbkcKoVl&FU-O?uG$& z_U&&BLwFVj;|;fQ$smlA#REu!IkvN>K=Q!q$6(A4w>GB`uOiX0xVk7EkU<4M6{zvw zbBnJy{~h*1o1SkCU;qFL+5gvJ4+8)IjO=Wk|4SU)ZLF_!FX9V0lE3-o1>IdK+C4Xz zkzY2eD@((w!%jmh%cG>+WM;n02nAPVjPwB~MS6z5(;rg_k)W7=nJ_3;&z!TuN5@&Y zgv7T3T)0W7;Yf7s4S>ojR>QfgdwTE3hRlIfi~M9& z>f6&rykNVS38i7V+(5*LyuW;c+q4HG=|Sav@y0h9TD>FO_~nK= zUU#~KIl)Buoh5<(1mRDvLXz1ixK}wnlBh?r z54|LRV>rKvtx!RH>k_qXTDGbNkH!@XcESJ)IaAR~=LE*zP#j|0Q2|p;MfLK-KUOIH|qU4A(kKdUtNBhN1FfH5{_tpztPH!yl_94@3A*eG;$QzW>*lvLQG zXY#<6s7kBmX_cL(kOyTrbE3u@UZQDS^O1d6APHw((gfceWwpv)Bdo4mrqEBino4=h z7>%aA`;il<_WkYW$*uXKeN}R`&sUXr%pmh4v10V{%w4P-sz-2WVh&|30crW4J~amE za_USMF4oRqd#Y4!NxAt;Mm;NcNNB6m*EnvoL*YhLYb)*#3@q7fN1v0JlniC83`7YR zQ>3IIS?+2)_N&Im=c(vf>x=jM7X&QKRQND!X4NknV;`fxG|db-dXV%BPAon;Gm}bW zFc!_wOj=REXclTD0&NYMjJw%2e{Z9iUWv}6wHfZA_#k#4)~u`U%bQ{j{&bL}@bXk! zT3p>VOd|8QI`9-;&Qw}KwG%|`_070slJIeu^IPyn5fTcvlGVD6v&py^&AVgwy&9kbk6nA1zC zR+;IFG*w27!lqO=dEllax%ti`EYPBu*=$5!jvaS-BwLIs#EOMYh%vWlXNvUMIlu3& zd(%@8D>kboIEJIS17$G%D9ZfEf>!^^coXfJ3q>0>P1xuZIr5kQj{N;X&RspyI*;I; z&cFLQPHiBUB6#?XZ-1Mep5393`SIeC**d*Yy~|q@*o3Ct7U{=cb8yA1E7CXMEV(rl zFW6z^$t3LpY)YHq6}LbN*`}tU)~Zz$*g+`yiff~nE}lhN715l4Xfkw2)p?Pa-cV&T zwr7L0`IV(L_BVs}uS>t|Tc;RbBN^u@a{|20UU4vYPjMW*)9O&b$$Alms3!b}nWl#n zVtR5$t(1}C=gkCeQ5yQ)d}A8)(mVdF^x?r{yi7|a%c|*Q`aMInd=K*7h5RK-)0>`N z*mu(0lDeUHR;POlFsm6|4Q>~U$_RsVtopLxB8qeDUC)M4l{^GqS`6!98HPqPqMv1T-B#4h> zF=iz)qaE|%cJEztUon-7T>pbUMPUmw9ehOAOWm8F3IoiF;xtRW#Ugj5YPbe=bwlN9 z(KoJrHm#gu)uDx^KdWC^N#RM1WwcDUBg!nUvOQFB3@KiZO!C)yJ?poea$i#C%jje2 z7unq=R~xMNIyt7g6_`;awQ^N62u*S`j#hPjPL@d|8=dITUU%&Y&Fwd`o6A|aDlb}z zCqoz8oY)U*v%lkT%Q;nlNgQ6LCZ~S;5D&<9ngBT;<`b#Izf|?UQJTR;_Nu>M#Sitj(9Ux-r-TT-gYVn>Mt#C+APt zJuVuZKMrkl9wPa?a29w*sz2XkiR6R(AFzQPszwOoS?*zhC-s_TU);U{Pke1N({FePDWk-Y zM8z|lePmnf`q+YBaYV;!6G0NJ(q-~7by%9E@Cl>H%rk;cE@?{5n?^o;KTZC# z8vB_>gJP|@sgqM;IqNOq3Y^L4A=Hb6OR4@V`V(j;dc>3(c_z`Vn(+qs$Pvab&;_<9 zo6qb_@0h*qsBN+st-o`8QSZJ&=<@6jI8C~Uyv5n)bOb+yH-8=>G-!q4x8LuHS()U@ zZzS1D6UH zRqr-v>lFtJRHuqFt4ivg`W&}t`pv=~xIMeGhsrlCje z`d{iU{9dLF{Jb&RSt=v!wi|l_FS`RRXuKH=RMRcW3!BZ7U0EKkVt*c{iG;lPJlOS+ z19|JII@adlsu!*!&uxdTxou)H*SE2mgc3Q1`2sz4aW0+uu@)%t+EezI|cW zNE7c^)iXzOo&{+v0@cc}vX=&l#ndbilrG)uE zSW&j}BqYU!-r1*qnS5grqW|L=meBQ4AviK|{*V>UEL4m$JC9g(xdJIlC6}(y0hG?f zP^Bpm&O&_ZDhr?^8~jsG-sPXKB&x`^w&HrF$wVOreWuh9p;08}k|bwna?4QRV!6af zUxQK$5wCP-&`3`pc21~naWMed;)j=Eq;87}rg+xrEWc)neHz4No$$9v#M_% zPHBX8igk*0`sRK1_J$w34z-wtkBEfC^jHs5NK6}VKp)G~_WjE8_iQGo7X~;LEEO!3 z7bF$WPf%SKJ==~-chYzy{5&923Tp@0xibFngHJm)ni$GI6Nv~uGNe>SNF;A0=s6ViD z+IXAEo)|o~vF%Q}w*_bKzyn0o#>r^i+IKje351^Sg0X?JMxAR28x2`PX4)H%#>`c7 zI$IZIpk*6H=fh7bh3Xi}5DZ1WpPvg58e%~vD!WX(b?P%ly04W8y=L7zCNVX2W<3!q zPi&0Wp{I`mXAA_#J>GAi9B}{mI(KNF+hN28YTwxD1vT*5i5^r-5f&F21ks6RVAT}& z-LnHFYUw`vB8=_ddKE0qb=`nT8f(hZeVFBr;A%s^mbH@wTS59HMCuP6UcC7MRy%IM z%O4IN-1kpLjVrg*g|i?OrQ;SRj)!B;CFzv#v;d##+WBrVa=Ot2TX>U6j}&!Ygioy0jBHHfFh4-1BdfH`!h4Xvf=Xs0 z3zNpC8iUL+jI8wJlQ+8@mKl7D`0htAVwzR8Skphrn^$U zl}ISb^X5QT971G?rpxZd1*01lDq>%l=8h}lv2D}F=tZgBuO|B z_%>imRjByB!0$^c*@svrt_*3xb#(of+~SKe-!-c>8TR7_+iN#+Q=y<6MkQ877L~Bl zbRrzPK$T(Xh0tJQIr9GF?r`=QD^(xIt9SI5Adiw0w~Y6`^Rq|_Fs^usl}rKS<~cdJ zM#pcP6EZu2gpFTYp|G5gj(=n!G`7R&vEFZg(XDy_^2i|R(h#-U183(ab{gwo=CRLL z4`E-d0Wu)}$Z`C;N-uK(iBroDtB?JUckkxK%Y9Y^JnR7@{MrI=e{6qj|Mw@5CtC5~ zA@If*pOLj3tfR;ZHeXIzfGx54L9 zSb8*-c|IE)gV!KTNX1N?rxZDP_~SIpJ3l zSWqo14>{^s1EXb%|uCE*<{MS_u1B-N(^p8=`uu0)Y_bA70>uUXSjv!I91G z3dx^Z`+^#R*r0*_ApgS?Lfctn9YhG6>&->5gUb@{Bek}@LHH3(BoG#qi71ykmpZpx zIbaL_OK}W5vxL8B2~Bgz-OOKqo;!p8UuEF9&id zzgR!Be*sMYacqZ~IKi;bkZS4vq1P}rUwHgaC#&!!sauJ6sk%RW< zqTl#pq%TWCmdvUOTb5K)_oESW^i(lKc8XLX+cL+pE*1oAG4$&GhuMHG1NO98CHW0_ z)M6eY43kgj83-2$SC6AJz1}cC4DLwNDVpEmd&Je>j$U7CGKghQQ7lgl*u2%7iyy#$ zwG*c22NVz_000FI=>HVT|8HMrhmqUCjurs;Z(srd02Ol=8$(+I3u`9^=l^?4Z*OZB zrXVK{2aOF4007`5B}9|}0HFUGFinua004krzgZXnpkan(Wd4yh5UE=K!lLaZ;>Dxh}?JgR_{u^$hdG)BAD ze3yw~WhZ9fZtlt7%wb=@CnjfS-UryEm&&^U6*3wx&rvk~qyIut`_*rYSvZTu*P9hjBT5=kXF^GPb z2?r9FA4VK>dX3!szPZ@p`N@bm5~>sGCy0$?J3qW+JeO>;>p=~9AGmh{EMq!fqPV^j z!Zhvd=wXu4F4qjYXvkbPVEp39@e?r;RZ=%pB_$7sCjVXujqP>0H08y>(w3u=ZZ`tH zKin_7^r9GSKbdi@G+)Lc$mvrUmRs)3__FEMt+c~0`f3HHQoO7_g}pWDmXf9_(VdFA ziq_2QNY;U^Ci@=`S3~WWt7rImw8k(Vz$l3!<5%TzCz5uB$0O1m*fdpj9wznpG2nE+ z?{V*IX1;h_=5`-~%o>_cK%;ZE9rJd-unExD)+N}{TCO&W?&eN%FhsY|i1*VhGn74F z>OJAIN&PF@c?WCXkeVQxjfoK^6MUNPUV9a$D7=VoJ`Vq$%m1c>!$luGVm1p6hx8iz zPH+x%URk~$K><6RN-?U-7BJV9ecLmEH3LskE$^|IA6TJPrklwx-V&>_iiMV9YRzZz57a$8j;>X}Ynr52{MSl?=gYEd>LxzL>|5Exe=q z64g1}Ys=367PA6@>isHb3iSKc#Ah{@b0K&~gz593gg(>_ZPafL*l%SRDQq1bSs9VT zg4uwB4;m1mwncC6&c|wDtVT*{c-O^`mB2!e_=iUQN7Q5pk}o&4aq@crSr)uXu~JE6 z$wk?eYE_I*JZ~51wpY217)-FZ2rJ69OsHJF;%)jWvd!w3*7&$&Xg#x6y^Lknke1fG zFH6eO#T_h=PDuq9xe;IvC~vRjwYX!1sL$A#Z>$KfS2|GM$Fw2GBiB^L+D1kPKUZqyS{bb`}9$d;n!^2k}mH(!a0VM&M*`~7?v@n!+mxmsH z^9k9;l}+S-;lBqTGVFledN`jvK?9%D&raI^G}b0k1l@)@>b2MFoIyuOCZ+JW*~K7I zB1*vfsr`C8C*|5e#+rPVLcoz>lsvKvj&h>YJf`1k{C(9#XM{nAh19p+ytnZ1WcmjY zifo*Kttv`N+H%b&wyets4Z2|{p2IKcXha?dv|l+*^`$f7M*GFG8Op=~h>5W9_?1Y} zzI53-W$%8F2}Jtgg4!`=ug{?X!aq3tb6_$+qFL1p2EHk=sf33|S;4!`e8x_ux!n?CZc$7P-*68H2{$0!ba{z zar6;}c!!BKeVe@t=p{zX=Cw~hgUV$aOQhQK5NBZY0jC)^Y1U^$N$9U;A1q>Mz}s|G z8PP$3wcu>OyJ znqA`DbY{bTz4J5u>OL5Uu=;K&7T_c5SBF+$C04>m8nc$g6Hh}-JY!R+7Lzg7>Dg0% z&;>>I`nf0n#p@O9zsBJp*-jHv2SsJs6m$k!MZHBe1hs2Ue+dwQ4bzU;ZRc zK_``>wUNpf21+t@cv_ZS(*C&yV3P;o{2vw%{gM^SZQblDiP zlX0vyl%zfeu1E(wVqH@LAPZ;gxq*7m3FcE;vjBv# z-Jt(P)Yg=L8lz2V0HOrJ|C~$w)9~7o0w4=-{w?8}_`fB*gSjCAK<61+g8=$Z2xknN zLI6YwKcN5SdDr`=A^BneKo%1G(|pnX(_pLu|DTw>kOS=hjZnA~KnEDC_@4>+ewP?s4Th8UPo zTb|_f*X~r$%T4Xd42y6$Oh#Yy3ucvXVhhYr1m{K4k_*PiKdvJ5sdh781{(np4lF6+ z*fCt{#~Xe$*oelPPP>MOx1;$iqO|n4ieRiME6g-=VMt&j;dlN-(kXJtL|d2;HZumv zw66PADa*DKVj55_*)vmfG#H<9HZ6AZVB@Jz?liJ1zR#Uxj50MX_Miq5Q<@R}7X+uu zE91(P0}vD;*<0U9U_$_QV)Dv!XCFh>lG82girf_XB3&ofk!qX2AVw2ATf8>|6dBP3 zUN?Rn7}Z8<_3*a*+qTPF4cBACC{Zu8k>A(ew<6XL8~2Vvuze#7s&#Wi%F_6OUZVLXs_R4n@LgMciA!$Fbu^y>fZ9m6js&cB+;!OuuOUfKMx10>W9i(T z73Y?-;+*22ecOelK>}|E=9U%n{mX7>_0l50$ApYA^o;)p~!Hk@8Na{ zQ)7E0$=jqf(%ky`3z#Q@paR%c2PeQzKRb?Zw#RYMd+Zjr*|$AkH?y-i-%C~PI@EyM zT8+;p{Mt`7VzL<3deOoeBTFaw9TYAnK&;b1$yDN0cT|88SFRMZfBW2EL1O^Z$d4uy zX;i?Q;mFd)1=_L&M`4~=i5QYoEqkjBzQmOo6Io#G%U;aER9{;gKze= z_W{yrYB%((izLY5pu+<2gk zQGQU<9IpNFXtjQ)-P}K+!;EGA!MZ(U9(&t)EEFJqh;!mRkKp%daVfPhm#TCM;ZDmq z17YOflT|nBdU}wyPt+u)q!$bzB_3>pBRJJEm?ZFzcInmVF)CBhtqvyR9V0LxYg1E5 zjG{iA{?)$|$CL{sU(~KcS6>7jgsQ_=$wIeXXJ(d4KIF3X{u>ZuGI?2lboD~u^sPJa z>Gz5>vcCO=r+4*shqf}_-DagrGYRiz{IW76>-xv;DEu)T=`n!BfrhKc(l+biQaYji z@6^lk-Kd0p&R2fRfj~9ShZ1-fu~CeIV&BY#+Y0<>Dz9&>((C}~7oq?6J?LgQUqXPo z?#Zg8TZ->(jOYV+{ua#+iMBs?aJL=YwY5~V2{y^GPrI!^sFZONu)j+V zD0J>m0P`sw^)y=DKExbp5aid9c?@pPHxl6SPc`WH?sD9iGdByuG7_|;NG0f%BJwJ5 zD~1Aii;xpt7PHGeB$Il;OXJRt9k72?uZur(D<`t4yys8D3=Md+HX1&>=PU?#EttJo zF05^$a!WN zTxJm?R$#Q)(-{G^7rnQ^pShJUP0u?(rp+%kmF83v0w4h7POJdFB1F3A)44Z5=+Jv_ zhL1;i8^{NE{9~JrKi*q03_2kM{>E-;E=Ah8Z?S0wqz#xUreMC!X6i$GvvDHuo$NHy z4NF(^9M>gWh1}%IG6A$VyvBcJFu=k8E}q&nl+oZ(8sqwS7|xkqE8b%7`n(-ldr;1h zyD3+Zk%T&W7CrNd>G)3U_IrVsdrc$-eg^7{nfMD=t(T{Q?g9({EQ-_pV7 z*l@Gx*KQ)`8$g!H_qdFKb}aP%w!%C4PS*r+o%5lzn>jX$<)R^!3A+t;FXXu#d#>~M zF9oY5I;ea-v13jOD&kf}$kD?&c z5tm!U|Gs0XU7jQ%d_0W5!^Fyw{P%$#2sGd4>jYkK0tUqtEWd6U5TWO3s-hroN6^0Q zD?-|Pi3D@XkKSJ>fI&=rYY>3YXKXbohTTdQ6)>Ru`!=DzH#OvkIgt1mM>_q~f+cYh zzAOUWz=zL8v_`*r{OwwwX;-YUmk)5LrO%PqA0~&RKotP&?>0%iyP9VvkK93UB6q@8 z$|)u3Q55j}bcg%zU%T$}8vTI~4NkI8cZ^<^Kk7#Z><^N_wzNQ|)fz8>0@Stz{+Gy! z0sH?(JVfWbn_Kv?x)r3qv?2Ijh47m=9m~>TM+Sg8Mqu{&CsnAQ`^Do|(T`iy$0*ge z%kKXM_KyM_s$u%Ql)7zuggp~*`K~Vx;%o5Rv;UV5_J7myQ*1uCN2XpBC_T>B=K%=a z;ED5W2ZrGSTIY?1L3E%V|9E2DU?C{f`l&;4F~;t=3zu!m0b+ljNNv!q^?1D4X(eKC zQg`kNM6cTdF-aofnnW*z2Cngel|YEA_;!tm^a3%(UMOf*X87KawEyrm zV|?=RTAmCANQWeL-C0;{x`6H?q2g}83D+^Qg}O9HXCv;W(Ifi*p1le&D*Jx#QvKOt zr+hKs-(reW2qnRciFs;WcUBiGlBFE5r0V)514IyXE9J>SIXM{RyMJfG;+?bYUP!bh zr8FHy;k#PDfSAQ>lW!MOqGHb;eZ}ir7hjM_7zNI?+JGPY_mOyuEj!^_a}d*`KYRtk zl8VYf53+%3cMNV$iv)@k-|A~u&Jz>fQ;>ZeZYtjKEWm8#?<~E2(fEOG?rxlD`5(GoJlPp5pBTo??fp-NlH$*=qd8`xjqz7EUMXO(UCGB=@|czQdh55o`1Kvu zR%*8V5Xns~z${EVtn;H+e;EP1XyrF|+gZ~?I@E#Bp3N*bu=zVLg|GGG$d4z~9)fqF z*#j{!CBO|l{X5Fg+@={&jTsA$a=KShzoiSCI(3Wqq=oa<$rneOvR|BawKMG zl4(lD{ma$LJOBydH*>(x5XOf!$Z(CXzV*pieD#k}hLd9|S$3-XMbR%4vj-Gpf3FO_ z(=LY?{6(_|6(HxnYR(R$mFCslMT_M>Qi6t*s$Rv^mwDo*)w=4P_uqXtykAQ`H=oKK zCs|J);NK=n`tr=il{+TyB`e*U26ZZ`6&s@8NPLD)%G-P3KSv1%;(;*TJ3g&KBhM%4 zQSaDqi%ES$oW2DIF*mQG1ABxT3R;2h6Xl^;ot8(VOx2a<6fG}8kN8C;@^8_YRKQam zd>^zWV)id2FH0uZ)K=!^5WSY-mxXu-L~~(CMQdoN1+aE6nhqdsJ9dwe%2R+)i_kwuMIf~`wDGw%H zRVPzsMyuB~Z~&(DJQZ%(#1 z3xGsK5U7TX%7-$+C8L7)bCl0z6uo3k5l!*5t0vtFgY6vRPYWAfESQu+MSyjQ1dT~G z_g&NQNaTaCp%^(P{FermW*q+ODvJ2L8JsQYP=JOo3i0$@U?*b6N0RQ6*GjHkpNNdC zQl>V$emejOHVgS(qVyK&V(c5@q!_HT^`EK*?m+of9pb@A&FXGU- zIzc>P=zc)T0p<7pToRlBa&#3-6R$}-!S>Q-O%cuTyDoMpq%u(R z`I~B=7Kq7#J(I;OGnhbcr+(9KE-6rJTzE5s(4=Fh*s2`P>;5U6820owU#0KOj#T-9 z?6xj5UukFbpQM$5<(#cgIK1@C+piBj>;jy6kvW|Q4^wXC5(Q)K2%40!Kv}e>h@7_* z=>`7J4Kq$Fg0rA$r5z`6NWd=gi8&wWP}U6>6q4@y{{4iUx3K5CcO{H^&Jxf@oxoe* zpP!Ew)?G?e_<*)*0;8{#9wGO@oh4H;)5Akeq^>%eoG6R=deZPlR^6gxUJ?-tX4wJF zSDy(qOg>ki#&qJb($+dYA#}4tBRdhxkB(0Eu`DA04WSmi7@Vmpr|fIOzZV$b17ZG% zD;bHPNYgzW(SM~~!j+yNR^$W_)s1(t+zSDo6RaIg{Tl|wDSDP$J=m?~XD@3(E(APE zfZhW7JJmBoOCJxe6}vw?U;W#Z+0_2r{u8o-Ie2qEoeVEP(78EhhS6)wER&9)>Gn}u zcB!<~6mcxLtpN*1*#bbVNRdw+GW;1luW#Y<#oq5CV^$Z~svwH<07gLpL?ulVgE-eE z{hj+xF76`8-DNl}wyXV_Uq&ZEm%2X@NC$+Qxp4#O&$^Mg(^X2cl1YT2G{Q5`LN8i z9~J@-bZ?Y?3>MPfT<|Phh)xMQQi+BndBa+Z+xhX7^7;dpIlg_r` zx|u>)zJ?f`J_7c%H&$oGo~*zSAh2DrR?iCc{(h)7mPI-jzMVcoHLB{SnIy*i`$|G* zz4@;HU-^S1DW;(cfxOm^IB#XXWA9|b1DG8~r~_du_1Ax;4?o_B@~=w|Y5mmSc6*Th zvFQ!yKr8&SCDlnK3c(0+@Vp&gG%(>nwjwebOK0zc=5lc}x(#WK2rgbj7FX$L3{!89 zTk|W~7@I$vi1}Yaw$hvWdTfHX076-Engb-3ES}t@JV&UKqYv4S1dQS^CMp6jRujvD z*T4s)mgIeul5!Cga(GsASyePTrn@+q`v$LPn&HSq;!qNZ#Cb(Z0?Z8w9YGBkrwEn0 zxJI5 za1mkf0czpVs=S6!PY~G&T+q6gk9@6w3`?@U(SQS)U?{kjZ2*00INsWXAF>1&h)4exmhWB=m(%icT~1U`H{>pfKO`sKL4i~LGUzK6h7 zBI`Z{1DD`<>LQ^J2za!+kp%SI_LqS&i=LnEvih5&g4BJbZ)fA7WhlUUV|I;^=$Rkh z8+L_lx=5w7i=`3w<>Ey_rEtiLYH3AHmKRN)PBeV_kNRp>R!aWz1Y%9L=`!~HW2i~Q#$xDNzGJZw-GIVpUS^S zM~5E3mu?mYjU!RBcvQ1^nClq|yjmpKf%VI8;WSk>z3SJazeGrjEZIqK5HmD-a9BDmG zH72qS6*AQ0zDmloY>AI2{T)<_cghol1z`UoGa`1|h-QJbsia>C_g@_cVFNud#C^N< z@C)a=S6e&ndm>LhmQd`773SQ=FKp3h?G3kY!$pOACZWk#A94&Ek|NTKhXA=FT zh#3VO^S0DS;8R8aOXVu=rCV{tkD2M>YRW`DhIUBvXoDWSyGcV!SGnmyuj9n$)%YOn z(==yZYrNiy!fNV*;p_fbSzDWvM!ic_=jUulCj{_*mT4Po3%YB+kHR?}&Xe^P3Xm#M z8yS?ZJzpXv6d^t?SdQMOO_ac-nAMyo1?6K6T4VB1nu+j#Wzb@S02~t#nptf~$Vlx* z8y0bOM}{Vfb5xt1Q&zR>GKR?iYKdzY>@6KRfd&SV zPGO$MrmRX9hs{6<<8WmB%6=OU0cXQuf)Zoa2E`2HP@W=-os7b`mV8*jrroPUDUm|) zPVF{b5FPVBRE0riwfyB5-CwyJjvBaC*mMxM);4r^>Evi~Y|%WLaQWkjIB+b!h`$;i zR@v$mtE)?b)p+`|TGGA#XmtCB0^iGPsf?G~>9MZPS0`GN-+f+pa;VcP@rULEuEwYK zGednP+;HKe~|Sa$qr4I#i7yT-Z(7!xA+gQ#Y-{q&_UAeu+mG4#wSx*d-`NZZjHyqFURPzz%t7*L zH-bIQ3mH!l1?bsip$SZu--@-1kMDt#>0O-BRZjF!4_<^iLkykn4}s}jy3s+;_ul%w zo&5Wp*CJG~>!qsu(hMAk0b)%t1byt*oilXA7PoNrRkL;AlGV(+kS7KQaV5=vAvcF< z^EYQU4Ld@+z;qRx7R1G)BUYvPK?#Crp+07dsnS6C zrZtY-%0JOjvCU|owZ{{euV57?@E8`0Vr73Aox>q=%7DGIV`%6ahS1tw4IPs$Yn!6- z!@J)aiTC2D1Ou&BIdprBwArF4O=sjy>AMswj$+9=CjK&uy))}+Wv10OlBIe1P1VPE z*cz}pDa8Gmt!O&FPf!IqNC45OHe`fSgk4qmBWsaHHgPIp&Tg^6S?sf;{svW6T2{bI zOucgBOadtGH$DIU)ZJNYaxp8d6Vg9|T4M&aTH3QN2qTN1of@u1I`EE?m?Rxaj|qR} zV@{$ar4#P7`0`+6mLnfoRtYTo^3ho?aM z^HF|K!}}7_Tie@GK24!uUdgfZnmt|XsR^CibRrG+E2J4F@H|sV(GYgR9L3^7+tTuD z1JeB{8hxl}gcvOfy<8RJaa2WH5RaD}0G;dAZU=-o>)ol-=r}YxwediVB4jC- z!x91p=7@8DA!wCBNi>$;LyGyg&vO}R%J`wC;2D3;>Bt$alUfS)Ms^=BTR|iCI zz4n1HW8x|lsP+}POruY{9XyyxAYF3ZmU%=5(~f~Gi{ft_`d>JGoyZ7gaWO_6f7V#j zz_AEWl482*pSOaX@=G?GioB-czCz;@8+TKdHq$6n&I7JQyuEwwp}#@F;Nn{9JgYhi zdP-)nFF#+C-rhcBfv`jw4703~+7mH&jJHBxZPs<9DjKc235-Q~&^9YAPN2K5HR7*= z21-AONm3%(+U5a%wKa?7NwR6olt0E~%35(U%eJh9(uy-~r^(8d83CeW;w;B#RB78oO?Rq@cNbP6OiuA@C zLvMWVM^rCca3rCSu!gkxiy|tkgfNy3#iF@DCS%)ru#nb!?hiZ^S&=ywhIw`Gpmhq& zC^*PD17c}4By5Icc>kfJsdTv&4nD>n4mZnhnGD(C1q+x3(Pg#|U^5x@z0kAZ8h3iZ zie^L5in81>VBiqi&s#L;0e>K*U<#RZ?#&mjJAE%5N%#ctJI(vk@d};s7k20G!*<~G zIFwanp~!$@t}2b9TCOh|Hh?_;ujxAF-Yp_Pq-<`gT@s6_d1G(l4W-z-eMt|iXDJD; zt+i=B!3&*Kb$ncXtX?VS2&}N(1E4Oet>u#A!N{OJ6 zG`FcV$m@|m&34_1iy*@hgCdtu1+dvr5cdiYQJ=0dJ=HhI!+enf{W0|EJ2!&2i9Tk> z{%M~iN~T|twAz%z_2WotwNW(1iRUQe$)h7)pV8PUnUn5VH6DT~(jJM9uW!sp+`O~} zcm8yEwfq~mpze+o6k(T2W0O*-O$gWbjGHgpR+vUe4%7ZW?=p_+f3eyBPKNgrZO zu@}(ee#Rexk`ypO!fOF`;L|O3Fe=T_qZBX~zVWr37FDfv?N|fpVQ}F3!xZA5H6z>s zm?$l}KS0n)5(Z4T#T6p0I*Qb$V@dw`@fG5&td>2pUJGptjQvNy9!J5>tJuNX>Vef^1@oz-jXcuCa{Z2LPF{TlnST&z@Jwg;lWLR$ zLz}7!XFw1JyPh7s_y*!do69{T!fgl(3KHKqb;bFQt2d^5i2IMimG02~y8ogmM@zWI zKT*(6flOi8n2bXvE?`{4Riq&xFlZH#-al8n^($35?C>ia?Bi9vxbT?!W0QV(=^T z=UltO7FF0&QPo9S-DQN3$gPFC(Xflw}S22kCUG6(x2 zIAJnqLbxyOcUs;eW>Kq(aqCa?(}-{*4b8!v0b4@<4<=$w^Rooka&a|3JyCn&_n2I; z7`vLodNU<&okLA409k84{5rLxPLtvf#0 z;$8p_So82qd-RNC_M)`|`jTo$LtfaZAAKEIR^?l8eo2z}dI2MlAowQoa15s=QcIf; z!%B7~fBTJRxY^`1dlTTwqK=(Oi^4~-#R+eRAnW0}patS;GoP^;1w%}o-*kEOeW@4h zg}BIZ-l}SA;d!ZCB8czE%i4R>>Njx>nA}5iEx|z__r0lN-masbl=EBy+>gIis+5m+ zUv;6{RfF;49p}#;gyRHh{nF~T^}Bs3d`k(8m1PwpZ>7I)uh06=w#==yGsI?f@PTP) z(%J<}lu1~UN?kmu>umz7eeo{1jTT&WQIylxZ=^nV*w)c%MNRc$+3Ia-5f@epd zZ{$)@qBI0DZ!SP0OZbs{1h4~i8T=4ei)!{Ls> z_aiy=%LYSVoVXyADRm8mBKi4`2jeUuMrsv^-OJ=v)7GW$ADUW_TQPO=SZ6L4_|WlP z9aOyx>|H2KwYTY6sXk(}R!3s&r)-VB&+dT*Ji>~qi?WKSTrN-%;kSgX(vt=$2UxX; z&p+&7thbjJ2V(bk^h;k#M7vY$>`Z9q@HE?XC5!Wm2@3U8WX34=5c(KlEoB{m)5NYr zA)on{|A3M6EG;GP_@%&uR$zfW@Y0ATcW0?=e@@hSH9>MG0LZLs=8#2Jf>cPh zNTp4oAc9+*W=Vjjyp?Ipy}&tV_@@6MDZN7%cm!BLxdwCoyeB2~+ytcR6q zYY;SM7F3mG;utP@-^jhS3W8=IYo=-sib$1~Fjp%lXUCPxz{~^lETjZ!5{jfaqM4KP zG63)w5*f&lBf#xE_q}0H+$*dDON0=%=L*T zI^R(?$x2xE1vH`%m*mmK#8i(y+>fdFtmretBm!}m?78SKC2E9aT)uA72+i`eI|;Kv?|;+QMsM%hbrf8nRbLau5NnfMQlYrge>WQ6i!|rSZGz+ zlFd{Kp5vG~%pQEA-fQ+^R^XBT%{iys*;<9EgpqZJgd zHgp_^mlIQn5@e$e>X-a+0eW>t)S^}c+DhtLWoTWakA3Rk*z~2wtUMdo zojaw{V!Ct|o;5CF-j)KkHm%Y2S-cti_a5YPndNPp#6~Gbs?2ffdUc1^qG(u(#%|{# zmfe%zFW8DmUqY4ihNGG2E`1GX zU?8tYNgn1tRj^+zZG8XvGNi{AvNna$q;iEkA>&y`yHnPauj!z$?4S<*3Z!#hU({~> z<^(|J_~FiqVA1{SNWo`oH>gxip|1TZqlfarUAJ!wC7UEO93G8{bjN*O$T?6(G-q-$ zGf_RYrbY&bc@c<463Ileu2L(!+L2xyxnF$4l|#k4kxSue8Gbr1`8TbL-QadLHeGxb z5O}$X9~KYNjGHTgM<4zFSVfAiU;H><^?v<-&cH@?u!75$=v~WMynoWA2 zS(m`;qD9RbQF^H~_gsy12Za_*sUbcNYb{^mTpK!3@6Mj`(){DRq+B+~iQ{Rur&I1Y zbT1{h3(T*%6pxCCkvpbb5jEqm)Y3D#n`As3#k7NJF{z$Zhr;YwHD3bBP@_!adP<~* z<-j_BJ9Z^#Sc+o1E^f$tT!7%^6{gff@I7rwhmn>q8FUXOgME9nZy$e*5qX;}rs>Jz zMZrH1s6U{va6|&^c87#Lpp1l?0VywDTO5*_5eC~wa*8x(+F=Zi$dV_vzL|h2g_ve} ztbA|k1Q+{{NVh0h?YFoh6vX4_SJdA@9FsaPYDgVAGt_I_qP!DSe97GOi#86ROVT2f zJ=5!v_;T#$`uci~gLgg&^Kv%hCq(Jxf|t7M?un=S?p*3IM5Wp{g)x*IUNIi4QU)#P zCiV)9z$KK&zEI;HkEn=MI=5dQ_(Z6S7umcDi7qt@-+y**>o9rzAe7pG&f2UJDy(k( zTuezZ%&wfDgn;kB_YkTSF-Jq$J&* zEAopAQIcb-wWtH8cDiz+)$+JjG%t2*QrS}i9}O{bld1@s(O!>HEc6g=4NI6#jEIeA#v|#aGLkP3 zMc!*9Buon}Yf58u%fAG9Oq&aY@oq;2^ATdzu65kOyKglAd?*)o_C8@}t zk|CV{s>=xj`+mbL5`H4DD(pcI*q6Z1Tf4LL$MvH=fPB0>C736gL9XH;vsTS7M+rwUH)tsNygHB)~3DX>_-#P@)ndbhYnTV~`_ z8xI`y9v=upW#LXs(whtG$@A2WUfcr3aj!L8SJ*~AaC!&K6dl~fe+skO4aNnzHB(lv zWb|6u<0pFNyLjh%lG0_$BRB$WONrHD-Vwy;KDW@23=DC3?z)=+Bj*<@9y|WNFKg0Z z_{cyw8;LNDL>2k<7w+;oHZXJ&thsKG+8Q*vje2p`s1Fd1nE4cS12z(}xu(dj1Mr`> zwTIsL(rH9y$}C?FQQ#JGI+`I%lD#ZXbCn`}2cp0&ll)rcu_>5dzMLkcd!_Zw;**8t zFo=Tnk^Ii*8FF%uM0cr8783N%YkhRZux9f@IF%{H#yf!_@41+iZC-HJW9wi$yx#02 zUd+vv3(o=z`1YRBeQ(%*-FrKnaY(G5oT=FbXdj2pp@I$%z^T<4(AqqokZMU4P_|H< zu}5K91>!ICZY>O}+i5!FbBXwgsgqn+?Z?n2ne_vDBZhFfbZ!|_9aLfy#>y6GT^1}ok^g|nAB>m6&q%F#pU&H%L zH6s>gH{_>oG7|Yd(v`lvUO5=#b8(z_c8V6t#`wWij!Je^?PgWEM!>;+MxpF)-~!{2 zhhP7QV}5-sioeV!p#t5zP@7I@l@UJQy*NG$ZuesA+Zaz-&8q(DRSivXq+6Yw|H!bd zC{fjsA||XsIQQjSrkrwoz~UzSb27o?QP*5KQ*yssRCTb1NyO&od4kk${Ok7WN9%lX z6Z9OalmgUu2kA9N<}BmBCvEW4<{nD5G#sYr9qiJ0yY|7NLG-fwvE~$?Ns@LPiMxan zB6{Hr8O=+E+58oI$Y zdIvK(d9d#HE~B2QV4*Xk>=W6$tLdiKPV*IJ#i>wbmO+ZURd$09IParcXX<0q#A{4R*t28% zgsaEvAK*jT2$SE|T7Bh|oJH>Qj-T_j+MuNCKgwxQVkgk%DPyP5q-c1ThpF?HCFTw< z4|Z~h^#M5uu0=Ns%DGM3)DJ)knqN+skG<(*IGRmeImW?0&%?poDas=N0Axf@?nP(a zaHs$PfTq&z8v?3O<*Rlud$^^A^N*0DLY+qYH>||<`+S$z{R^`RIoDu)uhPB(l>eY4 zBnIk;`N~q{+GXYHWb9g+f33Fy_pf$$>2U?OKeQd;^Mz5R?LBWyw@3|odKkXumfXT4 z3g(E8Pvdn?D1vt)6qj{Ds%S(c2Yeal-XhY~kHdGbGjTad0M~ZJUT6Hk!#rJZ$Egmv zKk`wETdG2145u0_y@bkx6Ze3q0>q58;Fq{o5BL+VBDdM{97UysOO=CgwCf5U2E|Wl znDG_^HRW!5socaLWQKp-9jOX3cfFf~(hLw}yC^ZnCJZHE%{t_bb9-7(hxo%z<4*dE z^F*AxT1$+xlgZzot~A!F(Qy&VFcifT@O{tjZOT{|pBZ8q_x@4TJivS_#W?_A0tZ{v zb3jVK@6#xM#Yr~h3F~AtevTXbahLrsw6KYm{|TV{GwbKvDr^+tDI9>m1{407`SYty zY_8QQj0t{8xjG@9_@zHT>O4gP-S3}x{#oUe1^D5*P61Z_tI7C_2m7_+r)0nn7j_Dd zGsJ%OWdBsB>zQzWB@Y2HCzAyrHv*13(V?<6#xJL diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 856dd55c59af22e3595048dcfb5dd322c8cb2326..0dfa43cbf3320e8ac2e3d8247cb67dac7b5885c1 100644 GIT binary patch literal 30076 zcma%ktpDM?&9nY2KV=C%U}xNN+I#Y`>pT) z-V618g7(O()OedCt&ySW%fIat)UTE3d zE5!;KWEy=mIH+Ni=~0Ix?PZH~1Lj~=tq82k#kYNlFC*Mn`O(08JD7^nyQF~Wwfs8C zUozmw3^MwBd{USTr+?khJfgqd@C<(tY5#XL^;WjVAhuWwYDe#^2Pd}lmYj|I$o{>q z83V)EnAq?_KYJfR;wle*F)nUiXQ>AP7neTeGeA2B!e|p|G4S!Y2h(JMPNC1{0M9Z) zGii5P6gdg*%eRrTWdNg)72__Q4qu0OppijtKy&K(M-{6Rs_h)$3~b%b{vkCZ@%jDZ)zpL-v= z<4q_sf*ptHlQgDN1>1 z$PbBO%b=}M84A#D_z=WY6brG`V`{_oqDj@r29N1N?o~F+;g2S7@<~#q77ne;zVe&=YY!+H%I((h(qPR?7n>&L72I@Fr;PP&2tntlpzsigeb#c8CI0bJc? zkTzocAswZ!^PEQIM%ZHA-ShB2fOc7O4hT4lqh#fKJCd{rz``DRe*NM#?o+^zAZ+Lx z-ktSVAbm}T+S|BWjQpm5eeD9)$>H-@{!7E=9^Voos)GrePGoB5Uj5fA>0w@#Z%A4IE&!aRHQ)oN; zds9o|FZ7B}vu*>k%u|{`D3|TNo_&?oy(6^!@_Scsz}Y31OVzN!<0M*(=n-mWsef3n z^A8`SsDUcJvG~gw1y!zK%hrE7WAG8rV8?i&*}j#HLQM#2F4J*BAUqR}So6vWILnzZ z5NyPG6^j(kfUm@P4MpyNv&DH8-DS$Mi;)pIw|x3PN-u6V=ADj9&wikn9||r!#*g&w zfC;nmzSP9efCcu?486ZX@EZi4@tlT#61=~Lc#Q>&9gd>xhkwp}zK;5><2u>|GLgSf zThM7=Mgk#~a|LED*nTAQbczN+oDge=;+Snpr56!ZZFm2KNrzc3c%B2!EN}u|@g=tZ zoTXOc4rT427Aq&7J_dK}s8lG2?fRQ4Ijn@R(;;~$=pwURBVI-aE^%1u2_=iC4`U-h>Q+z-)yJw)Q*2TS9}*@51|+Z;9Y zu+zS3d)U^MjOb^@-;{K(%N(rhKQq(x@U^CAxte-GE>m1Yr~JE|*FN@1&AYAc=?D^B z8A6|QM-Qxm^eB9`&BdH(? zNw?*h+SLA(@u0p3ld;hk4rfgTOr=aVtf|CRCbX=OCOx6;U#G%s*FdexoaT8kK)@6Y zP9t+BY=gX}R0?O-M8$WIj3qt?l0dkc+h?#mVruVWIni_&wzsLktyJDaE)SDn-@hC) zmvR6QSyj20y(a4zJ-MfYB)qxjCEmpp65-yxP39tp`rN+OeAcw;-ZYk9Yg2o4Kt@lf zT#$b+jK7<_dJ5vs^7#?%j}j8!bUKudvK^Iak0UpEXpR%+fY}?B!PBzcD`dDJHKGDf z5PFP#NpF=bm@tY>le~ElusjaYo1E)3m{vtU3iG8Qe0v&>7UVjYN2+|DeZ8FV-l9QE zP>bY{a9j&HFoF*Rbx%}uYx%Qbh@YYhi8?g8;|zIs zbwacd4=}zpD+SkZaa%3sihEr)>h9((u0vR#hEp#{hbXl(&t!%b>`($u0NOtb%dlO& z`JBx^{A`rj7g2B5U!m?i`~K7(Wdq>Q_m1R-ZjMxvXAr3L`e-hT^~%%ELGbouH`M6$ z@}a3=g{Z)?xrq8<#4YlnvG1#lt70F8q{kqgyXy~z)FibV^P0WcFgn@M`Mvc2Oe03B zjKHvhWljqykyiFYZ1~{$iAi~bU`skCLAQeu>oI%-trUR+!YpX@MwsaeA+-STvvHY+q7M81FKM|$GN&S1kmMLWAempfl8@)~>8$`TNtcOer#Q=nAuC3!xU!66Z1kfe z{f~tU{F-#ASMLd#*pv4Wqh`3e1q~1@ z*dp0`sUlqA{hxt)fAP&iy)PIYU!9HP}HMbZ~b$;JKR(6nOxBqY;ts5w^LCHv3t! z>2g`uQRs}$quZ0jU*AR@nh1qDhQ4a&wJ^^)rho09i3acA)P0(A9_S}cxm$tG%GL$X zg|@f?#Y-xWB4Ise27Y1l*4^x!L}g~{G6YjIRDwcYScsx*$QV0{~&H z(eeFY_Jos@gS()$~TAOcBgZAb05 zf(~-^U7u?c*{jk3y#zqr1C6;;DXpuB!vxFp$|(U2k`AatiJ&Ib6D=l>^F$GrQLDGK zni0+FTp^9PjZEp3Gs{_nK_cQ+)?TFh{a)(ig`7Mu;9zt?Wd++#m z{FOarZE@(<$v}nkupt<8lpKoU_GiOVTOzii*VxgbO@ieB{)$m8MC1!-tTvN%^l?tz7v zbH`1hH*D9{uocr7r|))k#eDUeK+oo);vXVlqVfcBrNY{VZthut*p1EaUi9Obb8v&C z(~T3)LVK_vKCs4#>wiTOy8ssZ7t%3<%boex@${UpGUm@mHFEgkVKHS_F3&6R>6P;s z{j?=#H3tF3gsx!THjWrv$)cK(`v;;QGtyp=(xZ`7pTfFGsw&bzfspfS>2P3OPSikU!rQk;w6zi9!`>qbj#DyTvJ|~Z z*kfBwTrY)sZO=A0OK}Izjz{=G*A6 z6+7J`lXD@u8|L__0ew| zekFnpom;X^wa$iGHNH*NDkM7OA02v4#Xs$@?$W_EqlmnI4~vMrNDuxNT#QoJJFh08 z)SJmNT#Q*yOh=X?10^SA-@30Uh*X0Q*6ND!nrH2=0j*u2h{kPcpUwH*ua$qk`VPf6 zq0;=QOEAb~!J>b^LMM-OU>#ED4{!-d3CFp8)%FwR-CRY$D**|TwP~V3*M!swO(@3W zw7IHklv5G$d}eBeckw>@9H9=)4;v0*_me5Qx`H3Jt;Dv8M4kr3)t;%OB*6L#EHX&P zb|fP3q{%7hNM!oBv27=T0}Q^3vS1+HubYS;Z#vBGq?t_ZS6XZX;+-cluu#Pbi58d@ z#Kyh8?L-BM9GK`0D-tgKBTORg*DdIWG2s3JEK`eQ*WN_ct*GL_V*4#ac${EE>5uyu zJEGP7YrDY@Vh`}x-A;+YPDi6%D6WA??!o+eIX?W|vICY@;&kA}z(dHMhA@EH`qDvF zJ#;@ewT!ANAT^D!4i|K!!6gkwYC!%7+i+#EtFKX*G6~9l%aV_$+P{mM?MUSIRfMaO zdOs$VcYV_`BhMw?(?YhB(1*YSq>mpg!nU&!GkOMkm)g$h(@SQ-5wmydZF4uRa;OPR z8++iU_G%JGY@}F*YDH;LkZyNxM21*_o>W{|Qib!=mRt-O2TIRb-FQ=>jY?r18Y}qLRIHJ` zKF77u1Jz9f6B{f~`o~wCC&S-KcMG-^M%LwSjS?uaaAAeStP`|Tho#>$Gk;Ba<~pjJ zRj#(`_bZ2`0RN0%jyt!^yC}&py)66qX{+H2hbqoiv&I_tdsq2dd3MT(~q zZidPl^4)j$bW>C48mKOs{4%8%(J4Bn1rSx>t439N5Kv{5v>TY^L6|#m-4&!Hsa^D8 zsD+c{?6}z-vRP(b zv8Rn0b;QNvhqvf|j-nH2DYIX+kyBq1<`9EZlO((v#ZEAR^9gOyr+T?Td@qyX1Gds9 zKt9;x|6RtxrAg`}$aWhH>ChA`*n}IeiY>E68h_}ft$`CcwS=<@<%Gg#MQJ8zKhw&9Q}10JN0lGRzkN%JMTr?!|+5kuSf;y(}WN#*inx0=bm1lBCBjz1lMo8;mR`C z@CK#0AL(Q5sQu}I_He6;Lw#*znRdmV7Iw4|J=G6t^ZPe;p<`!9-qvSv2W`bIjd%Q6 z_+0aIz*j_DQm{-sNhVRm8@};_%$KK)X%>Xi4)B0{i*in>^zQ^DE7=yKeP44{ozYJ! ztrWm1#mNj~1oosk2GgRt8Br9oqN=!BZ8dn^98P01grJKO6BPP@@4C-9nXstRq_%4w z;7>FyH<5kuq8#Sl;40&-ddM_bdW92@1j_}Khob007y6Ot>R%Q`(k$5@e=FM}VuqtV z5yvZpD3)(zEJM_a#1>B~_U1ugdG%p!9UT*onx<$Wvx5)!Pyy+9s|b#k0=>#OGO>$N zu`h?MGoGZXp1#_>$G4p-3gf>$47WXy}&HuKZG2Unj2VR|%^m>{sef9w1bMwUUWfv_TiVybZW z%4&$&UKU$YW3#ZEOVWa9c7t;!)U-nO1q)h4%blE-Q&VFnrDaF{+Kp-Go!F)i&?2mp zaKAx!-{c+8>A^eo`QI(6ZTcJN)zE;4;nVvpkxIFUnTSl7z34BkoN!#)_;RR9sm<~r&wBiJ@6R^sE8oO-+4q@maBiNd%hZ@R>EOOx9rE@ z+u&2Wo@zjhkWS6mTISp3_&}&DsXa)}E|;bbfW1L9d%(?Y#T^*Bg?n1gab941`aJM* z+)`Zz$1U!1dY^)2>TG@(`RVfK5iWiL2=`J4{_1(+VUwAdd#cPg81%zoNI)d7xV96pMum;EaS34W3$pg;$1`27-Tkgo%bCZwt%Wc zY-f{YoG(VnDX73lB4pouBi}i>D{H^oZ|k%1eu5Bv>i237p$@9Nuk~PSW9SGZH=|u= z8PV0ncayauk1}@s*ut*nn1XjBbe@e<#Y{-|V|+26tX=~^r)J^|uUHTJ0dupVn3%bl z;MF7Ya&NHRa^Sg?nXe`-v3t@r*NG5^c8yKLeDj$jhpUkb-yjYQc_*GH(FGl;--|n2 zSJ!rZ8gp;rbG9aYopY{Q{VCq8uUkLvnbpMC&OkB+D|3i4r|3!GN{|Dj! zdroH}VqsdlIJ2?lzNroK!Cy7S!~m&I+U;muc1 z-`&?;7suN)i^c48j@RrH@7c!eUz3qa_P>QMks_4h`%!Gi0I1+Uo|)4hx$dyW8~_P+kNQ=Zy7@pPVx8G{qSBB8{FkmA3a&eoW&i?&hkRbpsYVm0SyBY@6{l|0C88x;@MC%E?aug78+1tqtYBx1{9`d`pDW z?Sc1;CiTSU0I7Y{tYv8<)FxMC$%?;~@NvTL4Mc-waWQd$RD`;1fT;B*+zfIbYQz0T z>xp1H@VtH4c0vfMj@{K*_$VL!fmI%|B!aH6g z$h_@D~bAW$@U=a%jb{# z8x~8XkwBOb-+uQrcOQ_TeNe$55wl=-E^(f}9uaXE_0G&oST~e?pZm`53q&|5G4Wvv zqb2z>I&+rhB5)EhzYeO4ig{4=W%I2YX*-a3SbE3a2KV3!6&TDvsJj=qrwZPuN#dlUAitMO zLO+&%^m_#2g4z=~hJ9q9pH6keu8p(V&uTB3uaetVf4@EanI3x&@kO}jm0YLbCF7<0 zC+R81NpHRQ!klCv@U5v}HV3Jd(!h&2iXD^?Edy$7=g&bYf3c$g>Q zUkDmo3eye5FlGak%yIbjfLeVgFu(wuwmT3I!rvto{L&#%K{_7=F)@ip1?lKOObb`5kf?e~l zeqIEGr|gc!?Rw|#%0m(pcjI?~d3Ay=?G0dnRNg-h6l?qR%GA}!bHr!+PUoEY5W0QR z1SFIbT-%zw5DMb3=9zxTJzMt%Qr9AJdei>NEzcvwO7d+#rEdfA z5A}ra9`O&O{6c+_)*UkPi{~9u0HOa&5m3m8i|&A&S7<+Z$7zm<6_(^9cL3%lh{y-; zU<5}TG;rY>PIM9!6KU@&z?~O;;D!_GO@#uT!c%;`3LwKbA4E$nx{vrB+=13EummPcx0<9Io zU3lDvXjadd{RN&A!OII*{%4e6f7LGJ%Vb*zTEUEJx@o_E+d~97dp0U82L?DpDTt{c zBEF5d>mj1_EGTf_e~=N1{zh<%Qjz4yqV!~x#1DO6YfPSx-A+F5-R5m3^T51>d^A+C z$Jc$ZdS>bb<@@6>ssAUfPW>6NAtf1bi!?UYk^DbM&)q5|+{~1#{msUkzz7~7SPVlO^znS1YGjqN90#^M z5Bnjmq00z_imDoHR!2d%Uw^gC$>_&M*Gg6`Wm^n%46?%pOENqdh=2SdZ{FKkGHRw$ zXr6I~s%+F;og>$>ZLW_O$6=VS#a|QZ<(4-p3yW65E9XsHu32=|nO^QPu-dcV>M~RI zRxho?og0BTpK6)KN#?gqn0H8#A0G{yi{EdlbTanxD_4W%R7X#36ZYbagQM9qICV) zkk5zMv9_<2w;GP~Ink9fnE7Qy{ZTI%AI z6d{>oA`t|zcx5l;S!$8z2M-`2z2w?Sf>WKauIK7hap;(*&t5(}{5_G{={S0(ZRK*& z?#<2}Vz#);cUk?>+F4?EnK#Oy9+S;%@M`j2Wg=qx3dcO*5PYzld^UCwrOblK$qeqh z-R>=XUPHTU1~MbH1T7HgqVDriAMzxy4@NAR^;atvnz!5K<+h63-EQn?;c4?%UtCVk zeoN38r^!zLW9UnlfCl^@t;fcivE;Q6e`7ndprt^L}!BTp+FyP*l^(^8%nD#V@X%@mxp3U$b^yi7=fhe^??sV_-aEbPp$rYV+X5&d!u5= z(w(E>%C#&`5Z_@Hr&Fn;wn|4xc+GfuoyOf*0S*6}~KUoZUbpPx!jhcR}2^FjZu8 zg*FONG;*{!G1#HzG5WRp3bguQ`gBe0_t5d#|#2eh3>R1;h(18}S2MxvQ%V8GTf|;*g#oz{Z&4g69Z(>P+RJZt!kQ0c*;?p0g zhhQ8`CY8>iM}yG(JzPi)RUaYMCxP-U2t8&MmwdsS@E@;wkv;=-e;t(nz~?`)EbG`q zA!0>6Q7SZquy3ha0V)~FWC&G60$q)4c?OfXW1tS(%**62)rZ#M)jftk;+GYkXwSmd0bPpe z=xAZWT4`j%53YD>CG%mXQ3#GLw^ga7j+oh%7ZldhGc>lmXzTG^9|=t;zdGtR#==sjA*p8zXT^T3IgM1@_i#afNMYc zt@oqqFRt?iAkV>``%sk6{^;oHJro+Ts8@Ys^4r<(m<7P|sX>|$u~I=ic>@YU{$&vm z9={nj8j2)wc|v4Gk~D`_$I>y=-k%59q9`f02(Ca6qi|Fr*Qt?vlC#9e)R35Xyz3ly z$HW`s@&b(%k3GzZb4q;ObIScSm%ksV((&bqhrs8za5KONmjH@L0iAw_*-`K}mpQ*D zkM>{E4uvq3enB9cw5(V8v$Cna#1AC=^#lP;ovD1%_)~^74WE3pIB#-zXyQc_Z47O~Lt*=H zPYzOC4pLl?P1OeJt|DO#RdN1Gk^jV+fv1YMr}47BX>)4U+T}i{w0Ah!s`Q#i-N9ae zR*`e{^wi`Je5{TVODz*fs6tvw5;thSV!8ReVTgm5nf&W=cHfkL({(CrUf1c(&fxCS z*4|v6|F!$|Pb_YL_t;dIc~3BC;0mPRC?>Ev`oBecdf|MT@St5W;QT2sBCevaan(3w zLEtdcL8nqONHQr3l(7jIg-DVK-Z3aR`4c?VB^;!*=1DZMfq~Zf7`p+92jH*Y=0!A0 z;iikhQk&qak(TL}UGny0{nuqIACgTf4%f>7N{)|T0Eb$%WduFTOlCL%1zS4HG-%W! zie?M)I@(`Tzi*1(0P`ibueKOh=be!D?E_5KVs-XHAg&EoM?s1Z79z|pQyW)4-nZVJ z;wv+o1bBqXbU|SdCBJ$en0+GLzb#S_+O?)V$I$(A3U3P6ub`TSA>cp|RPQgZ`i0SR zQ0KC!DQx2K;Rg!FM?nTorgwGn7GXVQ?DuPaELjEt)_jJS`k|r>u}!mKbaJ(>zzZnq z_LQ$r?2@0vD5rb8EBrIjs?p6a~}3>VZJ(Z3+UemifRgN)#qCAZTrgw&+q6`22~fcr;$> zFx4qk=;YY(*41H>V;P>7(5w>JF{vmwz)R}q_n1lmI>Q-NzZQLUD4d9T$WmzRQ`paa zuiJl%r?Z$7tj8kUndTkHUC1c-#*HI*rE|_jpJ3S*xVSji5mvXPYwLn$hpTRpo+m(2 zQ~#{(ieGI6OWYIP6Vt2qRc@*0TyL5ETYUalBYtEQ7{-@p9vrVeEMmKHQKi zka7^Ti!h$p^UlLqMOP}nDr5Upl$Pk74c7MD_Y+(PtN+Di*PrI35olm!1|EN=UuxKu zEA26ap&RrLLhYPb;(?!3Q$l(qIwg%QjvFcZWLl4T+2C7@B_aCElt1(}`D`%B1{*cW z{**X3-v?qXU1IZ+wIDuk5D?S8MN_=kiag#W;C8h{C0ME$XmI9ih3p98;-#)J%ZlQ8 zJH0*hR#0=|J1=s$B~ZS@W(^JM`V_M3tbm%QRkUITGVQ-)&I1vndXXqi(BS3wk&P)~GhxCmzWkbaS z!?O>^u@L|lg$hXfV9y6rJuJ|%3*M`A)ZOjfZS(~s8#)A4%psX!-kq3QIoxszchp5N zSAu0t{zJ?ss~wQ4&3M9exnNd0l$^Pnczz35ehQJ;9r^X!J5Lh*Q~#E|@LqB)$qeeS z<#J-zru^-WYZ;I#XaLf=yU2LZAq+ert46AzHG*=GAwJlUnPh?k{nn>$s_?Tl4aj3@ zsv9-WM$Kp4@}^l)ujuW(L4Slizue0&{?5+)O8Kq2hrki^?|@8Ya>n6j+sTDgj$Nff zTd)!+u8wV3l)Itaw3`9eMM!WA#$tjlNOY93$E{9DfUuk`De!WADwUI5`s1vfIJ%7sL%=IjKOfl zB1$=EHq|bJxl&R)X}HOla5-UVG)DHLo9!WVJ2=CTPD5muQ`yOdgoBg=JYTP7LyZr` zscnnGgbJ#lm1+=8r%S$v7q6)`_>DVqkGYtwVuSb`mAR-6?b2ukeOcK@tmTrt?z^zsqv7#8z$-sv>5c20pN5KlTYD+H1&+j93m zIEBMr9^1-SL@F}2RyXZ5dODf1D{Dx^T3{M>+LEOS6ecXFh<8R7Yca*1B^ComgNG}d zG=3hh!D*Mq7>d@puGgwNf!De}gZg(abFTc#0%(0ae&1@i3g7n{8-05bdkg8??icr| zq!KeS;W{iL1U5RKPh2;34)@#MU*S`uYs<8>0v2nRYLFW4zT#Y0W&n1eGne+^Xq4<3 zEvbx=v2V(*c*dCcFNCi6C`dSVLST~3Hkb2`7F!(^8;39W`nD7FSN6=tHt?`lb`%qx zs>PbMtU>hyFJN2XgjCpg0>lL;vUl0??P$)`9I7us5~hR!=i#}G1YT~l ztB!D^`ax}h!=8Aoim{&O?5>+Luh*YSmLQ&n((g{ta zpxJW`i{V<9=+)n~qPM+2NrQJNJ(P1;!9BepL7?K*Ef6kFQW$1Jr)K)G;>O4%ovLzf zRXJF2;KDyhl%pMvOs`%Ak>k*cj;h62OizU<{J4%RZnL2x2MG*VLavL}8KeLn&aX3D6MNV^O2AHo9u=JU!eC@v#wy&A1LUZs0 zoR>|M0v{G;#4pmpH9x)g6psa*2wEn_`grr8L7tohXI{C8?Sr+Pzp|cfXKIL;2oi_G zwDqwMKMzJU?0Olsx|IrSwfA#S2FRHhU9A2#@BBp)uw1hm=gPse1>m_tu!}+_toX9o zC~4)i#?r{SeCJ4#2&ur1IY#BK~18j0*UYe+}pJPxE(Qt$B{l zHU3T$47YIhqIUJBe`9@8f!xGHY_35y2=1$FE^AJR26e50ixTUYENe#D9pdfU2!%XO zoHcl56{?leJ(B#;&`g^qq|0r|qFC(_$J@%OV~;+Z^-Ja7K8M*HQ0HqC0KEgy@aWJk zAO4^%iTs-7f&v*JgfYx$jHg<+MC8Yx#XRW|Mt1kROmKlUtwmMOmi#Ur$jlk3eM3z?xth z>ikj5GnvoRK~z|&nxiVyxzS9xhYB<)%8jXPqDSv$as_p7!OXMBbDJ5TMyaKxn)Sm8 zu^ziPfp$_g!nBcNR4%*XNNwN3dkI^e}@W+ZW4Qmvu{-B9ln*^(bQf-(_%u91{pFQE}@k$o`cR?SnAhR;0o%6D=ZvKD&FIC zhC{hZ?UQt_RRJ2|j%7&r^NonOFc8L`pU`=cGbKZqp#)%|JrC`dhOs1ycivZT+yMn# z82DSDukH%8o2b|vm3GIJJwE|so2^!f6{2yuEUk4neyqNnTM~wZ zTHwE3LuKNDTDGu!;Hjz8!V;{XX>DhGicKm_V6ZsAdPnFp=R8ntSZnWgjW1t@N039% zD%sFSkWCuDmqUx|is%snzeDe)MSGNeR?73++1{A939FXO2}I#RiS2+ z(rfU8@bnF|e;pGWCkGXSgelXyTYZ+1!z5}rm)z0&>gpRCi$FHA#FdPQV|9l!n@MK> ziXoeJXaND}tW|p>cRCcR(O}6wG|{9?k;yVY`Z9_bAT(L6q$fMHCp&Q16leu{w5e;z z&sPK?9k&iRs$%tpV$_5JG_hhHjEJ+BV15~b@%Q4KOnXI?jHugh3f;HYEG}q2JR$!G zn88WAt74b2km%MbBtS{ei#k3K=^z91fyKdd#joG9C;5N2q`I8#xQ>-v1@w}C@%i3{ z7h=X^`+bLO2po>2mq|=xv7g-1ZJdSn$oXv22Q-#mm%Kc+w!s}!U>A0nFlcK#T^dQ@ zb~--)Z2`3keLCD$;vdM5m|Q@5Y*s62Pn|61QP(=Ct*-V!L_ml_tm-ar+}P3W=-<)b zU^Z;>F&X(TBpl9jf}FY;z^owmT#i;gpEz$i=_+POyNf5F{S~?YcJ?xb!dxU~1U&lo zve2h;CCNZ=nWfavdP&`PN#0MRJF)$Iy47#{H8L6FMBiAX{4`) zL!v{HXsFWG5TQp|D-@q0%!%aXNO+Dvb)Pv9%r0HAK3cls@u}u5CYGSAQm?upBg{8E zMd~x8SV`9roIbpHqUg3~^V)Qfs>O2OA&{YV9E~}i*x;%yoPq{TZvsF4UXBmEbqPxm>l5_3r?;&SRR8|X z+Q?k7y{s^=R@D1!^i(7hGcJ}K26Cwcx5Qm=pYpk>{hu=RV{p~TrGw4IPAEfJtW#DPg z(6U(E(&cFuf&(@00LG+qUsDz)(CY;1t1@ZmI6_JT#zF-!*l~zh6AWcL-n4<)Y59|@ z=-!_{O#(liSwxWLPcwRYdjAX%33$-{N1PH zk&VBFY-7DcV)is4dIU&hwIAKjv%TjEFvqM=GNKR)hax4820tfyXDNJkc^CRkMV~f` zU1POSXu&o-rwcZ5nm$B|3d91gpMQ%Lfmb*cRDw-^VL7s&LC9+!E zm8CXZEU4YJ@iGf(bWvmKw?9(x`^`NSr~Scqm79}pbecGPy7TTiEBQAbV}b|7jJYrk!aE`H2UB_bOsrmSH1M>6jo{!d;*O>^j`g$kwmN<`442Ke4TQ?W?-0i;!T}N z6EVyQ_5mxZx~vNnW1L|UOoJ7Qn2gGf=gh1zbqn*6H>C(gn`Zbla*u+U0bSDV8zYbT z2aITqRf3bri^(Tb2nFNEo#|8AETmSAe zf<}{>EaOC*^jCTwI9&#%G3$x{AU8c$Vpzz-V| zqt-jKHPQxkb6G44NB%wu_;{1!=_E86_NnEh>Mv z!%I5xv=9QSDIj5VoMvErYG{0_lw_0zFnm54yruL=woEyM=aPjADF$lNBJ$XDlo^Oe zPVMJa`e9nyp^#Er=Mg6!ni+4_$f%V`KCSGzy**s?=I;lk2-T?fGd}lR$t_MyO4xUj z9xz;3_3M2Vv0>ulZoAoTaweDUjwC?e<>&S~#|Qc2%|}0cyJq0?{nqDu(>A}c&{4b{ z(fS=_(7IYtQ2Teg< zm+jMO;GjrXsk|J3Y6HbKJUX0)Zbz5&nXK&bJI|yPsZ zk$->AIqs#~i8+EwKOtz1#QGUYia>P_lFZ8H`uTMU9zK+E;D+<;ZPvYfr!k7hN2MoV zYPeG$i(%3;fXM{Y*mD{lA0=ci%%}ulW*WDgG@KBS1@g`Saz&y0jN~tv#h_T?@r_mV zwYBt%RrD=EfyG~}UOG@QNJEe&l;(roX4q za?V7=0wZJ&7Q{`^JSYZ0CR6D!FVxDsv_8Uskx6Aw$NL+uN}wiDErmucIo~+_R(mhE zZyHN@j*Eex!MVr5?(T0~=MoG#IaM${1KR zr0kp>L#+&bO|n3SaEU=6jR9*cTx1&c_(JWbv2FU^7)Y+mq{3)5wEsqRA#S%@>yiwb z+d%5NC`oCY+?3~VAB(mWq)M$V*}^P01PqE1SMJZ6YHR(OrRehw4Ow8_n}7K(O2=Tk6Mi;Dcwyx8j8e*K!e= zdxR)e`oaqUZy8nJXG{hUzv)7h9HIR}z%0uY8rADf(sY_$6Bz!V!|Ug_OqX4}u_Q zRRdBN*V4MJH_Zh7i%)a{RDzJ;8i)c3a0g0uzHplkriM;|Q=wNEPm`yyqR}ebuHDxB z{QE1RR_AC>ju#UO8C&7aLpwa7D*P;@`DZ%Cf|_L|vo>>6_6@`+YPH}M*vXYk4Q0(?{Z=u>%1aNqtJ`>MHdbi%XLCmS(|Pggw* zoP?lw3G{J>o-2Wr8-bELhE|dF5p+URoll%z!kQYNc|mk{X@FF``zOf~9aV8*^!)uC zec`Mavbcyt8%+=An{g6T>@G6tT*0Uj8U&Sjvkj!VLq55})fWmi)J+z^a19q~jjeD_ zP>gZ(!vc$eo)9dmcxYD#(R9J@bls{zE07EL5Dm~!f*X@AP7_e(iOkcNAU9z(PERo| zCYJ6|O3uS(yUsyqPs%z#; zB?0HR0960|LGezV#JUn7gwl)`KY$diU`24mb~JM}UVCIiCz;u-2ujK0uJN-@{1 ze>!4@$s4EIS4w;ft6hck{nN%RJ5K<>8rK6X8u>* zfXEz@R-ann&PX4_u=gvWQA>t4WP;|;8` zOv3B()0ccwT&r7t3Oo-Teh3hI0}bT5DL;^s5Rv1+wnK%qp&mtu^V1b`L|i;3&I12| z?aTgFq)+sgKR5LP99TpQVMwCJ*e9eiPB1GxTxwfdox4ytw=hKxy!QMe4(?eDmg6=s z3G9D}3Ln}bch_KUSD$iO5eN|-KoKJbu)E?9~M^U&dMQsI(^i3^D;;gYGN|hrRfwDIrKB@vuJ{O zwJl#MJ?P5>++ib~Ch{6Ms-%QH=m4Y}4aG0)j$-904LNGRO1>w{SjQXC0H{tXJBOM2 zUBRyFu0WSX+x#a9oYO5!o#Lj6k6vH7INm)Hr^^m%2(eN*CO8kaLfiH_q#@rN^IBxu z5}JiwQMo139XJaQ`I>uL)3welLyxG0Z*rEU+{wRL#`=ZQMouUfhSr%b#A6M=9~3@W zE9x7%`$8;kBX*k4(e{>)FxXa)7Gdz);_NH7fo4%uCD|9v)*Zkix{sv>VU+iu?DH%8 zl?~09mi+cR5O18vJ4ZGTzZ$L1xkE40YBMhKM*$cy7y{>lcOgEVHW1$>(ZiGfID^!gg5^X7y#$~R2-DK zF@e7m*>%pGUu|@HPFG);w2}8iCs7ddqtq{H-1k2jy(2NJMVLYjB6myDhO0&~%@Yu{ z6=gV_KO16%Vh=WUFOp7`U2MwH?%L$=H|uzKoBDoYso{UmQ{DiFLF#(QexccN)dFH4 z>;JG^nA{Q^u+<%Y(<5&A>vATC>Kew^@+u=Zd&>BmB%EGjr{U1W(|J~>7*6haph|8u z_4;w*L8xnn@rG&o_cQ6G#^#JMht})QJ?w5x+epG;E77E!S8Wbn!<-j{D)sy4ykBPW z8p}oUtcXz8vpGeEnw!-lR>U1%48RB=Nvk9)CfnR`nSx*J=e*MyDb;=HZ%4YW(DQ2E zMY9z?PTO%gy^5AH{_TOdF$?)XPOJ>=B-psMMq%xB;t{5J+9cQRmrqsj$!=sLDEZ1F z^1B6yX&WN+0}+VyKk@U)rTbn;B)&>Iz&&&$$oD%UMU#fdT;{4TKYL%{u@>Y(lW}6V zAploh@6xdVOIA`i;W=e!tx9FKs4i4h*85I(8b0Q-2{*fs&N#95>s~LukaaKrM}dUmKMsfwup)N`uPv;VJ4>P-IqS?4;!rq+)8c^ zL$;n&E?h24VJR8z_q_=9>nUA8f^P`Q5A7Jk3tR{fVPIqzpXEM<_W(o^SuF}Zw z=--uX8aX967t6YRU=CT_-%?Qk5tL#K1sHVr`9v|pocW2*5MYLGDNlZ;FV?@671QxX zM>vwR3Tjm1qtVT-P!8_WB^9$Q5zwhbAr)Grq|pi{QP!z^6CX8{+JPDxM`4CRFrq`y z7eB5rel4=&6hwo@Nw9~$$>$aXKmQY9-s<@&dwEO!u;?BB^o}B`9h(B>0wwm9fo>R@ z2YXl?W~lT-d*tL;#Kswlc3Ai=1O4mBohVsCnNYp@1ABsYEC%GDpL;Xb zEkxQAlHaI$z;h>M3^^WW>-L9~G&}^X38jW*!GlB$9?ZZ(=5F`jA$95`byOvqg;l{Z;FvHe4^NSRB)qR(aRGh~6d{y?g#Ez}C z1IZQXJcPdL)xmg>M0Q=Qj^;;2mm9bFZ9fHb_GFduz_{e4E(gx)$q_LL+b&U@P&rd{ zcjr#6OvIMdP;r4)t_jjx+Kf3nmE>LMY%{ZJT5u~-C_#{*eUpyVaP0N9q@@q)5@tzH z@a<^_Oo09)*Iu4kd%EMycZi6;}@)z$ZF|W6H)}hd%`@YeI&Q^h*`)As9(#( zaQ7>wMPa68Fkc|2`xz84_GkLK@LYvXgfM{AFrwyB|aNw^XjVZ z^_{2dB{~*xwC9{Hfk$7Af7B0u$7$|dG>1QvOlqI1$#vC! zjN3(GgcK&j6nfQ*aHs~W(Ub9{KVpeHnr8bjdoWS&ncmGT@N03}`JqV|7BiZ0Zn*8S z7k@;fJ-J?Q6dUs#c)xpiSM_x5Cq%{sUA)mvjB)$=%Cedb(wgB>#wE0kKO!0PHVIp` zN?Y1Wy7Slvx}_*bh(n0e&f;d>BHH#j1uq>>3jmy6xae8+KXKo}_rCgg)VEB*@rFg# zX?{2<>r=)ALnoU@B3E*|az>Wt!L2E;q9Yz5U97g88pWzLvD7qP@K*+=iTh|xFEPwQ zWhL5LL5=cLvZ?sb)JL5+-$s`dKaR4w8I+uB?7RB-GZgOk7wHvyuvfGPOs7KWzGDfX zX1Tw!?7>`@bR#$uY{`*`&^kU}19?w|R{- zORgI2nLwqnhMe}3jXg$gmmW2^Ze`NuBJRk9ALi_`0vm=nAylzrtTl6Pt_LjGaBUSWMPeFVf`n@~ zmB4b~1k!E%=!TS!gJyhyC5D9dgyj;9bm#gkFOB>^2_r-bit%r%3nXtIs|~FpBg= zD6FVX{lsO`mI1O9*eG%s9KAKaKvPZKTx|WT%fr1Ox!A7OMp2!7%RtwQLy&kA@*-Y4 zXrU3bjN7|vO;05;^mX9_JK@J6UCcf5VDLzrURDrSGsO2nT zDU-!Do3tMqSoHh-l^I4L6Pc(kfzxOGZQ?rc^g*p z*X$YaLA@u2YEpGqgQIZzq2Y@6&E+8r0?c-mP#dpJL_a4KJ8!->-+gmiaClZ?tqkxK zmhUX8QPObC5jtn-vH2Z^?>#0Y3&H4!ZpM@~`@g!pr2DJVjeNWKTxvk8-XHyGKFQBZ zXx2co1G){OEMDp2Of(*iz}0Yg6{&!N1YIuW4guv+ai?ExFlZu=GRc}p$|wjghF65>@Vz;CF$+1mae|_|o=h)-lZN*8Ayh;e#BU$;p!UhAoOdM$hBxD!2JIf$tH~@n^V> zL-patVM z%vzS)63Js23?Wm)ltc)f;z!)ZMNFSyFgiemXa^@qViK=T>s>t^?%tKPJ;Jo~JzX5F zusE;Em2*@OV#fQF+tO3E^x-8dKvw>W6T!NM}+#5 zXLJlce`W2E$2n1SqQZdO<1FZ-qfQDf($;ry3`NmWh}E?2iPDe+>$v8g!ssk-EGWHX z5a@qR^~9CLvmA5MsE6sMghntf zhIa2HdN~inoAEFU2)=e)LCUbsk0kNl`555%6u*gPDy+44m&MiuosFNeg;}BBEA?TOCG{;#;3UGV@wM(( z@gkivBEky&6pI@}PLO5wLzhl3`|2b(00FPCDo79;C4@$sB(}=&Ufyt-JG0%h<7;8o zC0SnCHJUxF5UCI+MyHBJjwZ^@n_4g}@((+5Gi#6HNwd#+6%9CX{GHQa1OX5#FO2Jv z%3S2W-)>HpSFor4E(wt zl}F{Xi8h1km8vRy-FY@HEF24^1)>cSlPyrt8GV>Kq)=3*eGNY%xH)o%*MV{WQvapC zG$V$9%)K(dylA{!$b0Fxpk7ygL7yU$8O@huVntoVH1ej()KRoUva{6ikjiU1v@HP1 z3w=tmN~_@gGj3@ZtFT^Im*mbOuE|u)qw@fdu-odDhRJVT5!VMw%9&gF%l*P9c)x!fvL4uqM^lVnEmqAfHZdxBC9vQXXaD3AVfJp+@fgN6Nrma7QuR`~v z4G~H(udH_NP2XYVgR0r`yEBICv37-*-Hib=U!+i?qvlay5WdN`%#PWSt*YwepHzGb zq&r_{Z|rDExz%!oZtY45{B z&NR*f9A@Kdmf$u)>#xvL*f*s9K^Po!UV@sW7&8!Hl*=h$)LGr_sh&J^E|QhE&Kpko zl=J?QfW^wol+Ehrxg65Vj#}oeh|}{lbvCMw_R6wepDkMKJwF+raph?hfshcfH;lon zD9IP(FVCjlc8K#Od`sRmxU#df-8r3yb&hgva}+faHPf1CUT|H#$|kyLNA`UEfHs65 zmdsaM&j#W#I#=x?T}J6rUv2gAJYLyJCOkhT;k7ogD&z$<+~y;t?`U)y)WG;QmY1v={_&+*MJgQog6=c` zCxz#;&JeYbgQk0Ut4pljX`1-!)1^A)*#ULiFmP$4%oh6)#og_qkcuB36lRfvy5;9V zSS6PbUNL(Y7``2vB-34T@;Mv8FcK0|N8R)V-X1hO#4;Y@iox*XsIUxs(U7zK$Dqf> zR02-)LZM^X;|W%=;*;|H;#nkn@5;$y-1m0q$|Lde<>|B#N+&wb)i$rued6HriRIFE8@KcU3QHsQH3qA{bLUn7suvd7IVojCx;_;-sZc*w1 zuZ*O~Y2u~n;u?r9wKpm`Xwxs!*9pDz0%9lm(_iLlhMt@uM+%F}yqpJhk!^idI-=05 zUy%e^YK~TYbEUBQ)J>+@8ffs6afisa;yMvSTJyzT`om3qC=e`2?yb zmEYjks-&5zocmQDqfjb8i4|w_2NHYBTa=rT3EZ`d5Q2~l@iUNv-~M5Jw%?xU3y;b# zXLQVW2p0CrpC_4#bd9YftSeGE+H71LIG78zx5ek^Iy_rLV|tp)4jS)GHj(T$T0y5z z7$3{AK5IF|W%L~ih~~|Ck6hSmBel+xo(jOUc45SbE?aA)={UbaJX2`@d7)z|TBDg; zedcHMfPTc}k>ZxI?Qz7VlhGjMGE(eRD(xP2M$PN^xxBku9sl@{u5rPUa^(|aW4`OJ z&a0xUCQZQ7Lb*o!MN3(GUu)j|@`Lb$-pqDGuEzUQyyYN#<%l+RY=})-z8hdos7Op; z3qBJT95cXPupL4|J`my~D3fLSq@f8+%(OVTXtKvu?*4YT2GPK7{Z_nJo~?wPUGU`% zkH~7!odUB%0fa8oIJJm^07)zkzr-px;!0;l@Rw=ro0&x_syS7v;S`|q8oV{9>(w;! zkeu$-B~guMNEfmh8xj6x%Kt#XH*2z+^u{0h14sNIoH+{{rGn}2^CR^ zF-^or;lEA=Q1>(r5WEmBR7D%oeTkM9D(A3srtXrl*SaW{6 zsT}Z`pOgYxuX_*NJv~Z1v#Dy&(hkzDXRlW9;~&Ce*ALq1-ho=ST#WWBcCu>nnvdEmc~&gjZi}@?aAhgb)Ps0IJfPVn{{N7iFE^p&B`GQ@OikH)WR7 zp75LxF$&Oa(lP4hTr<;|ZiUI8Yn|BP3OO1%p~4EEDbr;q=tk($H6~8zOK@!}8W>i#E?%!R<@3NS;H$z*(mrpcDBWRtI zTDPFi1IzD*B4~8JN7=zd#b6DbPP9&ZRuHy6{JyW7oUpBBkb5;EW@x-S%xD|!@(>Bq zi?F~yEyqzI%un*CQIsJg6AwyDA}lMWKlt3;hK9Svb@U#Ry`G%%k+7xdEV3`*{;@8h z%%$ou67%Wc>@K~#3)>JyRY6*GYh}ph--{%d9%1rd!`T&@|t~l7wv<8so28960=9Jqi6X?0g(gU@c11 z;9bAQ>kfa=!I;{>TJ%}~N$uCH+;3(SbaVm<6jCSk(cw+%*?%iL@5!Sf%G=s}(7)r%&R+Y2;}M9|DC^Z;;-u z+%ogekjSI7^F<<0jpy8JjlV#ALgfT-h5o|M-*mZ-Gr}iG5#5^@l77lmjx^|Q6M1qB zvcnI7ZNXSj5A*c52sk8TvBWYH?y`j3?{NE@_?;Qu$$F8GGi|8Uy!2r1^q`;SMzfzv zb(vp&G1mlBhha60X|AgdQ}tpQ&K_T^Qff{?U-obv*em2%2>#eXxu5g zSRk9ogBSy(yS1}g=ExAo%W@Ow`+uHhIWKph1+ymi|p81RSs94+&ug2qwCm@+!M9m%n@Oy^*-N+9> z#mf_fl8t$1s>Str6~gJLKvp}}CN9jg@hvOD{HM^JGY%S`e8JNSKLZ_3%ps!}no)t+ zHF(^~flC`IvNP;$_VJi zy+=FlQm)yjlWu|8oYn2b!-<;y>rSXS;GmO@%s^QN4`6qh1`A2zs*E zaH+y;VM+KjIa_%ksSaE&XfGm`D)F--sitFEv8Of%eMkvZgqcRQUum1G;G!L|to?wP zDqP+O_VmP>>5N0-dy=S->E$~#V`+LD0Ti@d4Z(IVyT};jn9`-;@LX}NuO>J4*FtjL zW3}?{DIr43idbHK!svap9J{`9kaNk3D%p%1w?WY!5LnNWvc<#MneagO6rZ4R_#v=9 zA9W+mU)lIZfp-bsp#b|pgxz&^K)TH5()RQk3?qI4ctXq~W3k0PkRvGZKcr&nddq%x z){s1OyU@88&&L`#1Jt`x8?q8DH{}~aoh4tead=FvyZ4}Oo&-Bw^^6z!e#kK#(Vg4r zU$>NbWKB4~?BeR)?F->DPoOVKgyT#f`K7bIE;ef!5_RN(=$cWNhT*8z0|Ez5+}G3d zjwyf3v@z0xQ4^!AK7DxnWeU7`IzBKg<0^1GPy$8R_D*J{AGyG8|9V<3x4jK}?f&LY zg>C+Upec8*KEwXywE9508|8Y$Q}7$rvG*5DOl;z3c@OeTEvT3Am6Q6d8cX6Mm-a?O zg5ztR*;$WciN1*F1V)lcD+Uo7P5Q=t3sCc|pgih$T8)W?i8eb?&$D~M&|%TTvn|IG z2zhO(nObF&x#ZKCQcXQw@-&m>O!h4Hs7kkTUQ+o7rc(1BF_x8*eQql+6|Oq>%aA{15VesA>{8nWocc0lBf7M^4ED>6m*=w*Sv~#OrAl?cB*Tad z_Ih`Rl<#AyHFk=3Z*0}?4sOJ8Ys*T&h1KJ#YOn*!!{BHh8kmvyerJLy5SCnGj9O9!^2%^gw0%su+Ojp9bTB!HRj$rr zq{*A9_tPz{-JQ<;c;0!S{r!gf@>MF&Yz4RW8ePV=^YZph%SyWI(Wpb}j{*7QvFL(r z52Th`S+#IoA^eD}gskZ~XF4fD{_+RkoH0!F%L||sUk1a)v?vC{djX{{-TdSMfi;iQ z$Wja}!g6rr@rFidbVa^v`iEErFp_erK2RqDw3CPiGhi=U1t?7}G=uM9QqY+1HZK+~ zKi7EIFw)}NzkW|z%xh4kzB`8uWd`aV3*b6NW*Sm?%G%(~7bc{Rn_gdN!b#BJS$1nPU^mJp`d5j)%^WDSIg+2!zu$zwAR=Y)`np{n0b)*o`tM(w14gl!o@L7;NDP>*0i{5@7xpYE1{)NbCJ7cW}Ml zB%SN&ygwmd;J9#J^HbGpeUGS2SOg-+v2wk_>ESAve_)gub|c;00sgYJyT=Qx8_F_Y zTy3a_u)92v3E%nX#Oy1Ot|lob$p?s@);E#^DDN0gq{ZZIL{)`pyhH^NB8hLnwJuH^ zzk zj%+!P66bWVe1R*2LF0Ly()S2+dG-D7TlezE3!b+-FumRc6)zgTyMEEUq0S0euW|&HIs!Nka7xq;Z~`t3*(YD6zY+S{rv0_kM@CZx1Sy*be;h6hMv$h@ z#TbFE}#A?qK9mU*7wG)5hnDQ_=535QF$`@ z#<30a6e|OqxI~MXhfQqy2idO|lnaulrdz~D9;V&mk||Bnv$ya}e&ufwn*@97$m#PK4bPet&G>g;+24E3JDMO^9? zH^X7~6me!OW-4k4VcuAx4yT-6FED>>$q*D#!7J-;lWl~Ei<5EF-dq)s=CUXbk5<#m z`oo>r@J4>WDw?KwXe-r{(c9m-3R>}35XKLZ^c`mxRgKLc2^qe4nO!lL)m>-vVuqux zO*Adv)TvbTBK_3yA|e_JddgClc`AZxOe;RYj_t?nVQi+}D!MBMY!{{HNlztbN>Z}V zFmJ(Mpu^&tU%XX{*{hFLykHfCmzDe}{hqWt=hqx&k?Vw}I%~;$;~LgNp*a3Mi@=Mw z~+KrBfqG&ix%xtu*92*yZWH<&crTmz&n9-4Lz@D$t5VliK&nF6}&NbamTba=mk-55WSVde!~0u$QeYlb|z6Xu=bg}c`FFhQ;v-49!=qsJx-3W+2Jnc?rmccY$JL8Y2uEc zXEk;hXd||@KJauaSMXGsm&{L`Z2XyUcMCKB7e-C4m*&2IR1S8$v+@*K#3wYVl37J$5Ip(kSe%*Mt_E?t)HQua}jIC#Y^obi?Z%CHiw&Z|v%G7|brHO6Haa z_w;R(t^{I~Tf%3s3#bZCOg0&-*kWr4F(GQCFEGs`%p*=-jZzY-c4u&DA}getl$53B zmt*s3Hi*NGe1bqy3CXZ&`_@LK-s7wDcXO;ppyv_Q5zp%4xu2ZBB1S1L`=}#$_2D4d zs(JNrQ?AN^_ExdnVqnXVU>k?U(YoLh?dwKM<|}Mm+@K83$8H4LwgcVMaw>ceHi1de zfm3oeJUWCl|2__>_1d@XoVhWH+Q|V%E|uY36}TBexFl|)q3_<)Df_FVW7o)GDNKe^ zsw_BF@rmQ%^Z9OYd)irP{SW0kpX+iym+u7AGcYjxt9~av=-&lA<(;jdKp{&DI{~Pz zo{h1Uou$ohQc}kp3gqK`zNmR^jCIT;)N2Url&^*^6OZMK#dKJ z?4AV~QGs%HP;*593+N98LdIrLFyOh^s4OZM*T1c&|F`vjcI$$Gnf{kvT^cbX7R8`f zLRTFA35VIRn88b8uVs@VvN#ViwnuQU5_3hud0(n*9PTGY*2&^#>fPbry~DUMXDx?+CCn%bGl=%?NB-RSx`U7@GZcShu*xA2zey!Tuep_%_tC-o}QbbOciFgG%{kNp$2)5jJeO$Mn!d1On_5!NEC zvvu9EqfMEAdyFcYCwV%~qa+9IeOfAX_2^q?*UG>L+WS@(wQE;S+(*r`W5;Cg_Ga7w zZp!YJV)FaehS&F^ZJ3?yvF^x^n!m)4$w#_ZLh&UYRneYgFAD}%P^2CQj|=iZ_u`ua zTBfT<_Jog`_b88&za)==6Y_FbBCeuOp7o{1boBZ0GZ_M|3QaX=_h&>H4*-Q*HyByw z?{@zKhUx#-o$X&}1sb zKqX^+J0n{)W=0Tz84Lz6F);y{=>Hf$(<}@Ozj=&|Ouye?X6D~~W@hGp(X0>#)MvTp zIbbmO_xoAynf})e`eV&!`RDO7pMinl_ss|bq5kIoIscjeJpOB~zhoF#SpOJ5&wF0u z&vh6XnNT(UNSA*nFyMEB%c271Efp+`pWD^}ekaX;rqN%;`#Cs(LiB*&UjEMskbmdd zvtt3jC1g>7(l(ZQa!@-pptOJxP#)@Jr|}=@`ERM;ago+BgaV~?Y@V~8`FELMS*We0 zy^S8!7VukzEKmZfZ>;ltnmnINED$h&k(E&cC~RYCZ^gj@lmp7!=vdfVJuji>4CI#s z3P2r<^`NrCd_YlvosB(|iwnqaX<-Mou(Jg`gGKglr^Vkk@&Ch|=Vl0hH;4Xj-4XaL z?Jev8VBl|n0_gwflJE>r0R3-A{fAfoD#Xme@uwUJC~s_M2IUYE6cPe~7#TnyCVmiz zi4_E5f`C9wj3ChSiOfLH_50AZQ+tk!EKpfR6##nnF94+RS5$u6Bny;;I{l^jZ~psj z%JV1W|5Y*gFU8Lh_`fQawt+hQx&6N>es;m1djAzTThxDm2le0R0sIX<01Z^2xUs&i z8sPW70l$rSX8!5)zbT^sTRH3Bn)r3>bj&Obe^}Ms6Z)uJ2PX8XI%dd`+vmmuP^bl8*QLEc9u4OMh*f2ftbM{5EDHJ#LUd~ zJH7Pn^`QUyUkC(Z2K^CHCP5JBnPy`8^L~!@bL9VuHUtC$3xYrlpg-Tw(f_T^*wR8k z#|}y%zy=1~E?!^s9}fC>VDL8zdAT>vIVMg~TJ0pJfF zn1%8AKzY6Z7Ju?UAkgzB|C7hS`s|AT;DK3~7$N_~12M2NF#R`=k(Ghrzj$C)&@*!X zV?7Wf>p#{5Gcz!Q|7#wYnSu4c^@5ohS^mLe0kMGoTb2a^{)a6r5XS$u8O*}W%KQ&m z7RG6TC&R+qP}&wryKC-^rbOZZdZ!^LLV!tV(75 zs7h)j&r{@zqT=*S46Lx^gT;eggDr!3u*^gNB6}k%SYBR6SyMZ67Yia5A^?#xqqwDw zi>VW%xQ(HUsi>*3y@@G5KdiHhlc}LCtVi~imP{PpC_>L{?P()u_4yYFBv@5(3g`|R zV|-A83ZH(`3&FTV_M$Qv2dfq<^Blf9Fh@R*;kBX68x zehqu;oSyk0LdnzGu+w{ybe*b4Zwfj-kKw|o;;g8UTvpDc$96;h`5T$h{o>8-MmG}7 zTOY}hCk{px=AK>|3SLnMj_{Hhgu!q|d}G~YT8QCSvYedM1mI?ejg`&JiAa*H2VM4Ug$cZP%#w7s3d?=evYS*7lY!%2C zVOvLfbR61$nHG!f7Jd6M!GJm*a1u0G(o38heMIRp9FKRTo=!fCFj&TjAH+Yv=oM+w z_b`#`U1NvA)`@w^kah~vJxgoj{g+e4P=TZ!6OJraf9IU~o9 z0)7%Qy}B#RhXG%@ixL-ubjnI2A4c{1REjSJE*&5J9oJ3u@&->TW@5)6m6rp)sK=#> z)$X*+33QfL`&!PU5xCZ6;Jzh_L$>_hHPEs(w2pR3;gz1|8C))4KKalj*f2ikIHrNtc`e6RODPj-}a$Q@6;XX|*Ru*1zZ@tel4eU9sA9$MF-cK0+^ezz?$d{Eq$X5$2@^kc^^vBcv}maUNM-V;MCFBIzV2RbSpRAni_vx+^Ev_?hsCUF!fj0wk=Gd;NI8p zOyIBK-*oMk#T%6%)$YtfXa;nJ6K%>Mj_g`@mX6_%h=X%HqP7`-L*&tS14A20rS8;P z{01Dhmts9h?Js{(`tc)SF=rcf38e_!hkl$zj z@+{8Bk2_u*mx#R(y>k)J8k$1Scna|=PZYI7r6cLotmrWc^R%BqXhbe$j|Qr|(Wg+6 ztlS5eo<349_s?HEkY(WbAjE3mMS4bnBN+1~b*4BUE<`l* zBi+VSv=dcxi7>ZDl1Md*Oni+ASRPS?7iqADXU;;!a_p4$V3w=zhxp_sVoDZnq<;51 zC?@GR@Pm(%1Lcs*J#<67YvgRJq_H?TfPrn8coHI&Pv?^Dt#Z!L6V-B!kxCzhAL99| zTC9h^ZRgthGLmVvY-h6v7IK?87IK^2$1VC-3g$LgYKyvE=q$bevHocfI7vaMZ88rk zafE`3BUC+jfbFawo1^k-^1+OPm$^t75D^c1jx8K8_dHbKx+|(qVIS*kTu|ibZulpO z24T0svsj_w(q+$?{RWv;Q6pZY63MMy#|zV%^eLwd5!i^1p{_mbv5v=ytJ+bS6Z^aX zI3?aJB$Pq7Ah2n>F{>|I>oBNo?>O_zddbaM5&bJISZWDtg`3NHBYW-H zME)FM&UFJ)J+e2BjG+H7gMidt$x2c!VJX>0p!l3KZj7*9w0^upC$r%PQ2WM z&5$NM8>8-stiqZW#baGJ`$~eIh-Y@3u)CWhdypqVQWb!0EGB9!dKz|6WnqaLqJPEN z2uj_FiV{kO2I%?!W~6CJinAqgOXXlWakGo%uCV0M!GnHHAXTd}?|ROAD^U2Lif?b) z1UY)2YB#1U#>K40*p5tB;UbcS`3iMLt7WP7#Z^NRz!_Msg|%{MwM8Z`f_vO93=-(!t9PzY)v3g+f0frrIlT@ZRCev zg%R~igY)tF?*cD$`ouy;oP+(Pa2>~y@Sb%Kf%Mo8kdjfq_vO{c4MdcUdkt$|%qBG^ z6r`eEGZ=|Q^N6Gk%VJ1cCxt_@7-)4W?D_I=dm6htbS)DXj+K2+zr!&q53YBW(J!5n&N2H$LRwk*j!tLdS+t|Q-(EHPn z&aBGT!xB|%h}prfjoXY5acExG>v!)coL1%yvfmNYMbcpC9Ck`R|L&BT^y$tNCo_8jF(Wm8^l8R+->M{`Ar6je0cIwuq_^ZKu)GnU2T)PGW7& z6cKaXivD_FC3y|^$w^7PXSS&0x)gp|DRuhrL8jNmi6flp5Vxg2xHrFV8fvmtg;;KK z#`{e5@t1mN+hYNY>_sUv%jHPD9yAZ8+BUuo;aGvsls?Lp7kSXibKp^cfdvo8nRNd} zVbUwLBwHf%2wqON4gBVM^q6N2oSASTGG(424wmlKw41Y$ru8F|Hq=<6Y#|3WHQLW) z87xd0U3Au{9bU4FH)_wm677;qW^5PYCsy6B23w!sambgS5U?1*Mv^62M9YmBWzF3y zX8z_b_hJ||Gfdk7r00&SY(>}IEpGlxo{f5EW3qJ_p<(1AVUg z5|1kJ-!ctELP7wbm1Xrf$HkG@R9UbG*uL>SvUW7S*Wr)W{Pg)LEQ6uDuSnqsBXSz} z0+AXFZ9^n4>r!Ghx`Qd zvlD6B_&xJwxZTqo+&mkeR=s#sLR_@&PRu|-QAy@}aY#rh-HOK4w&-6mxPzu)J{?m-_zpW4j;Lb=Vzo5>Kr6cU8F2 zYrF}J4SY3-Q4IXWMpgbYt{-g{u4&+;&cpra$2fqgK1$SpM4ziUgALwf;RnU4X_0zF zf=Jif-@l_sxdAlL?+}jXg!uXz1<7u90uDBy=nZKt%~0ONpI$s=~#jAjBb#;Id-- zNCg2sb&ypxtg*Q#+IYsSc)gCjYSw*#Ahze*zXJX2AbSvi`S= zsCqh>GHNIoS(zHUz%r`18oB%@A#U$v3(F`hZ115>#LCP>#LoI(7aI#R0~3)hETe>z zy{iK+FQW>hs*|CevxA|NshzPWqlgNlsHvN!v8l3zFry5Si<9erWaj_!9ZuN)gY3Zm z@3sR50s;a8`hVzx?Y|2a5h4OGu)z`$5iuGY{#VG)|GygJ`0scBQ?fY!KS)-!rd-@% z3v%~``l(G{x6z#pI3$9}K|SaWI{{G`5#<{xbO)-3^J*{QKW{N ze=PnjxkV>~Oxy-@W6-7eTHG!z2i=PvA0O<^;F-QlgXFChe=$0=Q8;~i}+gIaUX{B+DvoTlL@XxVAMPDYmxuK zx2+TLIZm)*V+Z2+3OZ$bbfyjxrFTpT0_s*U!1yMH^K#j2l~jz9IL1+W6~0_;I30Xz zn$yw-;lhk@M0m@V{$`|DUj73>EB*Sz?2&P4pEe%_;wh9^XQ|OmKf<|K!V)87i;c~P z!kG2?at<_|yVqIEO_`Yuuf_H0JtvFghjp&0Z&n_!YA*}fpZ%8Nr-?-80-kC{k#3C*q*vBOOL#W&t#yaV8 z!I;WKu23&LqKNwnG-sgSNx8!0HJaqjYQ5|(RKU^Snz*evJ<5Qvd(w;3^oG=1{?JBN zy-Uod4oqTTf({mLm@xtDvyzl^lgWXTpO*_QT3D3{iUHOL|130o={}KV{TT!WCNNF8 zoXLs+tVPZA{-)V}hLNl;Y{nZS|7RS0{LOFECGj8MS*^?!=lSkBD8*ieY8OsjA7UY7 zlm}E``$^gZJY-sjlonyB&@Z4lN52rds+{=YxNFH#}^3R zCtNe2q|)?{Jp1Htmy};!KvmpD{1PYUuW#a##CMDK8dYUue+i1A3|T=eQ4K*f1Z18T z4pKsOn(1-ZlKaIhwJGp9a#)BLP(8li+hM;vvRD3u1N!Lmu8Gc+zh)HH8YZy}a~sUD8S z=|KyI`sNIV&V&OaG7$~^GD)a9z;)Oi39aHyTlbA2gv(awJtcx3lWXDP|8=6SZ0D$S z^Dom&R&cMQ?kc#p#4!jE@i~t;Lmh$kQ1aTwLn`4#VtPDFI^jQ*ownth>!9K@6O7et z4;CLpPqs5@l8;IS$o6aTn$TcdC5niT6PG5>we0x7n0!}&0>+iv06yAg?b4BRxztG}V$%rj>n79Xot z@ZX*j=wYAiPCqabm~49fTCeKmxE+6BK-7*Vta(fF@p)TA*TtLIMvbLYT_FrUG#D1x zCk%^tk4Vk-P0I0)YA-!(*B#IsZLZ+9w2Jkoy}v=VmXWz!>KOip)V;OBb&%I+k=NPD z!45LWL^ZIhvz0r(3Z#V_cr>sAD81s|`Kn`q%VOi`{9@R6jdL$9X-_=(8tV$MAKSK@ z@;9IjT&arU&wKBgr-moUzz7T%kuBi62_PV;fYj87nM2rR76mGq_RxgOuXD^>>h>=Y zAu*uHx>$+=ftCp2h*1R5eB0e(qMNeRcrneA;F&$dFPOZ+5%!JuTafMFK)JgI&D>R* z&UJ#HCj+QbDH(;?<=w|nz{8Bgq7wMLh!IOwO@FWxc?Rf;P~0P`<$~YT-fMYzTA?qj z2Hm>8Wzjj$NYZk8-I0|ny#f8Z{L<$yMv(#E@T!&vuBRL28Jl{ZeS4FUUz0sdF~e2J zMQ;##c1~+swv(ktT@8Gd7CfiIAY3_6H*57p4-_A9@AaO}XX}`MW-z$+mCbsg z#JUt9A_`x~v$cX*{1Hw?^V~>LM=GiH4j0M&`LK)>5t=bSnQ)?+GBN_k1|#DS66JIe zun;#v7&g+~S8}MEmbllN%UMVVE?{qJz0v3+HER`+#@~Qf?Q?`7NV7X%yQOz&KQV-Q z{AqVtKe2hCKVW^)StS1l8u|~@{D&?fky zUjk7AI114wim;$!vLgC#rFpe3wXPj{&|i>C!t~2t1#@1NEi_tP|5SVEOK8kqe9hcA zz&HH-eFbj(dlecpGESy4Sj>+-b3N@?I6{W`)8eFZw6`nd{k_13KKrq-)m)A>$cLuy4c@l5wYKVZnt?*y^}-fT%@e^7MfzF< zK|5pH^AJil5o@+k%hGk{)V+p}TRA}j7s=2xB;}z@%)*>zq@s&NN5aww{PusL#1r+C z1l?mgJSiFrQ`~2f%v)sM&VJ3_29m#_;J5=(gyV|L9ay{|@r4=pRc@CUTtHk4^hp_U zLaxOw<`w45$O^d>oxw682|>^O1HU2|R09b>Q1}93g*Rm8*-nY>4fOZnnm~1DyFnFn z55s|BmuNSSyXo%dUY22YV)0zO;|F*OxI=mTcw zhxCJYD8IuY`%qhpI_JV(*-Z(V_vrEG?tx_pThq&Tob^<9_|ZhTx}e6p^P(BZfPf;<2OG7TZ^5fKqZV_-KLaNXM>&-q`sXX1?c zFLe~z(!AN6A)jSfF+p!G1v|TgI$iF@U7;j6b*kYNyYg`FTwd7tqsX6d3nUi883wTG z6x*V9tkBcJ^Zbwsevv;yHGgLzugU#n_eo5^x;CZjWBfw+2S9ZKDGxM@7Kd0bzkksD)uy8JBv%7Slw6SBsox=524oSsV1J-{V}3-% z6FmSjLp^ z*;>=J$GiLV!Sl{OX}+}#lN^d(J$>9hB|ls5{K+OzvgTBG!(ssM8}q(^MzB*XFHC;I z!npbX`c+6mF!O_qSDIIyAo~HN{n0zrHn?pkJpbt4KL26=QFEBUILSb=tiwJ^;q@`} zL&`%vCwTtRarHy(naLvuCOd%o^s*B@pkjK{?e+42e;@3Vi67L10o4tfCuB4P7>pUB zQb_S7DSm&kBe0~s^dW>d+&bhw^fJ_mLRbKwZwONNTYMSt4*5z4>U9GWyAiV8hqKEN z5Oo{!iqbd5+XY@bWDT0Mve$V>YI7+tI184#-LF5GUQISM!W$1yftfIV+n&vukYd8q z9^;bGI_{o5q{+#X){pLJ+|FGSVSVz?k*7`m+V$G^+V~p&h0h12Ya;9okAGtHwf>XW zkIkR(2m2=jpza~}Dylksv`!g1uaJELFdd%E^*O^~|XqBq3eLn zx4Q@O{w-bdXdcK}OD^p}W5FhfvMIw@(^p*wQXn2yzg#d5p~DP$A0~^C@FqeYkC6Or zpU`yO2qOgbGwu@k4AGWFg25n>>+gO2Cglaq; zB|Y}CCX?NOeGMf;AwO0|j>II};6`SV{(zm6CzCc^`7payO}40tu|%p{y#6u|7Zcg; zdF<+&!=i*JMgqZFQUt~Mx!S^>#efQ?lh!lVb4`|m&s2&go!6jLvxbtj6_g8cwh978 zo9I}q90>`rXggl>bDIEr9Q1NFZf`8!g#Ha|8wgGwTmpdj2Syw-w_`U`hx1hCu(rfX zT;R2v_EJJZg4}t4QImEvhy`fbBCh3DOgE+x|%)G35kR7WVCgFBR zwYJ2=g|U`#alPU7rF>06YR@2Y&`a$3qn_u2`!x!;du(=h`_C3pt7}v3T7H^#s~tP{ zO&)jW&z4Q*%0FSkTe@*vm0nl)E-zdn#{n0xZSk7goUMr3Fm)Uk3KzBUH#RZx#mskd zaUe=NRfESxq8E{zfi-1j3xl@;kpY2Zw&36Sk$>2Yq-!~B{gSqWWpj2M>)f_mdcBsf zv%M_&9t z&W3HOlV*eb6PR3$TgkxG8*pr*&z3G2<&T@K+@Aek72WsId?V3M2xJf%oi#2$FZND! z^Eh~IX`#R%|8N&wDko3Lj)s62MXiasK(2Bjak!?NBIG=*-XFA*`1J!UY5S)6fDY z7YZd8w00!S>Gs8}WzlGGhPZ%hFXa9nPoZ&~3HM+^g%vN9-J|p_Z=;1ta(aer34~o>%D61?r!Q zDQ@#lkKtIl64l_`5D++a(zs#kq_*T%yPEUbHEBo50?N5)+RVHgoFQ^T9O*J*l#{TL zxsfn=bo>f$0}=tROUZ(4t3W7v1_k;$(=-Q_AgK?v@&n=n*u_;T@h#{WyB+ndmwG?b zxUa48GO};ha}OQ(PU<00RjD2XmUgtRYyLqA@?QQ!E^m+S=26MM0XY&}`9vBd?WkMz zLs*F4U%~!otx0|D$q?}bP*yQaTW}B3K+X>U`=*hI?VW%bHw&$J#N#Li6h~L6QZ^sO zUNR$L5f&cad3Du5bHpD?kRn5=B4IPPk>HH`1=#T2zo*ppFx49zlSeEv&geLv1zf@q z?m^Y;FrCGQ?UxMnkN4`i-)v-fwkN5Ls_zS}*y36xc&R%drCf|KY>OHrlo+hxgX<;r zQ8d7k!NKDW!Q+)=Rjpg!rr(UbYR>l+2M^-)eE>u_=f;`<#go+0KPgN&AcR1rA77m1 zJBCop^`fpH27Ot9B=KfH9Oaww4h8B+B?Phid}R&HphlJk{2}z&!`6|Yz^4rJo)297 zD?}_?-mMfIKZ-I7VzA=`cA;|sP_Ad!Ym+vOTdC+Be8hUpS zYG$`CNk2-w&ujbHKH6*W+5tq;`SJ0sV1^GcN%?H4`%Ifg?lIYgoL3o42yl?8U$B<) zUfOlm*@1ur^eBXJSS5NA{8}-faWMMFmS2mDE7m4Zw>-lqDR=y3JR`XeE3N@P-tD!{ z@AK6V>ehq~GFx(uu}!-;hYtg8N|lE9Fm1gEZtdE=V(&@~^TMTT^^3 z&9Doras zNf#NxKZP?4&9dRfF*M)$5|y8<8`#=Awe69=-W0c8X3gV;WR@V4g%p3!OKhyo zWp?6OIW|e~BrcdO@Xc>CadVX$0^T)ADBY6ORD>FyLtru^@P;sg|K`h23(BEz*uwfp z|HPlPP?{E5W)$M5@-MYr;ru&owo8kZG6`BDMm`&TZruSbaO$utAw>E7@Ja-~27On9 z^0Jcu)xVdwzy|3RjFOtVx>r@ z_^+PSgqQS$kLnE{QdiW^NevTMfm6J(@$JK%k_qCr<8EY@ zUGc(`UcM?8RmpCpI}g38L}g7h>>AJ$L_&4fTtyOc2|E#9GC5+%v0(tUP?C8TyAZnY znwFv*oJNCARTV}m11fR_9?-91u2E;0VN~AQq{U4aD5eHqA&;1)=CS5qI{D^bIr*of zX+fA*4YSAy@3#V@;H)ak-mkJ@v{qhi87U!85XT7AS*uvswE%b%xvTsI4fs)mW|#5f zbei<X2qMLh4C(_38RGUvC#^5zVDmP}5s)P~JZofBHs%TxamsLduQ^>hP`=H#bq&x#Q{Fs9`?~$npp3hfQ%w41XH^sN^MU-(~`}@_` zK>v+~K@AOS zA}*zFA{RF=jl1l(OO0A)mZxPu0)uvNBsq8cCTn~eWT7RI0D|ahS5i!ppdLt@eZP4`uI6O$6w_khbX7Ayp+d0{-UGQjTPB&3)UY8%2o(2GD0a4Ep7 zF@AhXUf2V8oM4HSa;_PIVnYKj-mrrL{E!TOv8qy7c?bQOlwaU+K$lL|GKonTb7P2GSR>NEIn^QCNbsU&DWKdu|wiu=yKio&$G9u<7oU$Af+G_&{nu)v-J z2I^{-cm7RqS@HAwGM1Jj=j&|pu6HYBXI_2N#kw^=tu@G^hUe!eYN75kqSrN(ELLaX`ytBN(M$}5D}2Gb3O2q^Tl6?iR?6V50^kr~J@3XI=n&3EG2*Knrz70*cS z+$%k~*;{OSjjf_^E=S=^c(`*-X;2>42n0(u*1ei&Ip5~T4A;7sDteoXQl0O^BLtt! zxn9zoyE22UOQ4fM+4neqFPFx^ibcVfExoRaH*+{ZSgu21GdN25S)J zP+;aS*Qyf5`w$(|fJZUBHF)XnT;rwWvp51om>f5ANQ{QknFBguWx;1|KYX?zr^-*< z$X2fWVV8N3*tw7}5K_n=0ArbjfGm*A3vYj><-l)j~=u zK{tO7uKL#dSpGcfjRyN!#_PpEfAo~h@n{^rCmn%*VZmhvagezG-6`;gYND_nbx!XF z&YlDpworYV8kEQF!p?mT7yiC$rjuSzCfj|HHeUaeCx z>oVm$eK+$k4XcCGHSdi%|D}m{GSxtZtt2vlfO`YeEORkH1f3V$% zgWm1sv9mo%7A<5EzKWQeVlxVbSfUR4Wl+d9F;wU7BA3ltt?kklrQ%3lnLALIy&(=+ zEXuj?n^;dw0xZM8g#}^TnZ)^pQX83luA|}eR8*3y;3)&-!^~Q7I|i#S5P^l`J@Eb4 zbNxntiD9x8S~t+eQemRUPh7b1BoRHjL4AV*fh1s-@#cce2?4-s4*2N$ot+r_%^djC zC5TT{@BZ?0gja9hByoKwNZxHdmjR>-iV-<151>e}| zCvJdQe1qH@#|nOpSt$uRB$}vn(ohQk>ru;EzFDmqECuT?h>(r*2FLcOcFiLON;HPL zeI~YDrXEJW-s6SPdaI_s#DUo$H`&!OzOx zt<~W#_~_vdTk*x!Mx?)b1&&FK0p55-?U8jCAp1gCF{bk%ezGQNCWa;{PB|p(P-<}1 z6Eu^}1I-vrJ4FsXpM`V^rG;(Tnqp!hAym{?1DcwG(E6%Nr_89GsCk)3bb2Qy{oDx< z=O!5L1}$NyW|u;-rObRXfGg%BGM&C|%m@Flt1)IEu%26!YN?^e|4bzHva)7(Cdv_D5*17k!+ z6|f$PKKA|lCK=;mvsr9Lt~T~(p`omiSarWGJij0Cquf@I_Di{V;V5IRsd>G7ixG6W z&DJ}FR3D=9l}12xS-VVntKbh__wp(DH!dF4#uPKzK|BOZ@TSplvYt#AwR<_j!1^!+ zkE8EHoD#H*4loaf_l+i@NEmPNC?^t0A|uDBE5z9n3bZyWtUyoH_R5rB=Co#rRuQaa zmgq>1p$Ag!_Bqi1{NeoN7G9n8Dl4W^nqd_ty=2!l+oVOe3*o#-k)#bx+$8=t~j%NfN00cC0pe9*Zy20V?KDN|h{d>9Rjq5tvu zbI19;zpv?JYg4d^r(IxQRc#een|7$?G1;)y;~7F9QVdWvs;u8K+OU*a z4bExNZK<`*mMa2pP~wxcG&8nVTp7+_kEtQgvS<~tgLB-ryFc{Xxw=-A6mC)qpzoM} zv(gN2L}NcikTemUnVcz6GoG+~r?HejzE8e(@I|4R4OoPS1Nlr4j{!AeoRAw0ckUcF zEv4TPRpPS}l_}~h6h_}#&ff;%RI$G?@5`sbpcso(O;*FNA$d{XqhZ9~fL<@+m`zhj z^p@nMNUYN!MVTKFMKKne9~Dg$hazowEwV5skuu-6oSSn-4ocaT9&&L&FVml=6f|1y zu2gx3Dh0k22VH{~uh+_QEB~2_D5M+vgu{stqJ1d4=);SWe#X{GP|$^={t+JF zMNkkA-8hI5h`XN*K0VLq~bE1 z_24tc^CLOA*>E%+MHp?S$KyPuvZAogId0ns>GZIK4#rTXd#3Gq0N!Z$+1Cu5(ev+D z{PYu&WK7p^a{Mw)Vr=`x=>-R!XLal-d0OV#&KzuH*3h8au79SANJ~fqM1!F=EctV$ z=vG=GPX!}}K#OG5oEz|(^ZDgN>7Opp9~Jhd!YkC(PZ&m<3nb>B zAmnXs?n+MNJwVbcv4`-Et0*9oU?ytU2I+e#l|CC(NF0?v!(0X}4r=UogR}B-&~jDb z+HPS!#?i^`|9enhQdt@G^H6VW&hmP?_EJ{075)2#y2=QtXWa!R`;;W^V(UGxZB_@^ z#=?*Qge_S&s@4D=@H`##p!NjEahSADUpal7}_t!&?=oG_{?5?!IexNP;Mzq0}jsk7INX(-A5s)ET_C%HAu8V~IV8MuO$3}SF5B=u)|POm*siLsEIo3TO_>pdNw`o|R7vj| zo+^b|z!`O(!y66pM#e13L$3eTPG=$;{)!DRB000+VHaq7|9XciJ+)GwUx-rY8$6qDTc)NIH}JW{MLS&r{{U_cMaF zhcSoWPl*ZNhr@v#6>AVSf<$lul_Lf1ts$5l+<3U2CG|AWT)%JOsS5&PxxSWxI06d9 z@6H`my12`TUX(Yvp@*wQid0nR?l(8ED4C$wX>d>3209gyRO-OX=MPG<>x?ACmBza0W6j}bb*_O^M(a+|Gz zo4(dJovQ9}zqWFaK8?iux?KE2<7Sq`nwcRklhZ4y4$V2T6VF#hA~7c$hqOEsR_P|u z-vK9pS%V!5-$3U;^CtTmfO=CLX z+l*j#i1&1h(EASdtn+k&D-ivHx7z5rnA^Q#zb&G}`E$&67v~lC{U*T)r>*aQ^|bSS z-mGpYXpEBQOi1C2$-C&ILEgnSt7V99iVoQc6~;i$vSfLERv=caWTA}Fb|DfrXb)@O zzWw6A|3389{4)Ekz^o9AYN)`82dC;?0fzdkdhg26l4!gc@e!Uhs2X+5;sWcXZLZ{; z?tnK4zzl##HFU$QXJ6&?}gW`0e4mi}0f9RFIUN-76=Z^pE3 zfDxTM+!mqlZIe;J%ImF~anUy=r8 z86Hn1wourN)7zXYCB@y=>OP#dzHm>zykg7M>po5xt=0PIEB$E6@wK2lZjr|qve}6t zo>|fV8kV`~_6x4*fT=rgFjzJPnJ&%R2EdvIv?@_9QM1s?Z7LUtSvHJeFY(Jtj}J*X z`6V(C*NPZYyC5^6b(bSwN^iY>30bmHVq2( zEjejqN{jk^6b*OvDh~7_h_zl*VCpe+$J7AlCH0CkIZJC9i5ztiVTo|GvQ4`bu{bGG z5vF2AgFuN9ktSvTuc#cu^Av~_3!*0zIi%I}-Km-?Gm2Q2t1{({1_bZ51}8ERSv%^i ztJvL3Di+4}NhFM)qdv^L+WL<;dqdpsZ)expC%BfU9*I&b5%aCOZy4Um4@FEwV?^sV zqHm#3?X6W;2c{n1#j)YIz}V`)=&iVA>Jy&Rm9Kd2GXT>rW*T=jd6gNjBeSE9O$l8E z^xiQ|bo;&&`W(g27Io;vjNnsKji>396B_5`_SE)uoeLDthz3zzBh5nkdN;Ao`zzMD zTy0J=_gF1!%V>6tCW&=t9w+~*?aD1R2Z_6@5~NB63#20fojQ*9$}%8P^|JGVAQ{48 z>EFOZP@pOMKj@GdB!KOJd{Hjz;?W}@HqO`~!&R2b{>s&xCXv_7S=E5o z!S1+0HW7Ow{VOy8Wriv&pL;KxGPw*&F@Nml+{ z$mV^?q0{VlwcAD5>#}l)ED~jxyyeLc@RzPka5SQaJ9q>eT2v}=A-vum)N(~k2idFnE7UG-! z3sZZK$Yu)TKiD=1s>i<#pO~DhJ9Elg?CAA;D&t&0sX{R%USz*$3z&tvQIAd6c zuy;Ka(bVZqQVlPkA?74{qQc0!TuO*?mLcY{ z7zfXyRG>*TZh>Xhz@b2DjoBZGEM-;7HNdtcyuiBYn&O{sSG7vU!;np&wQ;V?+*vl%3Bx z4l4St!we;{6*T1 z-ObDEhDTLfuS_az6qRjaTY{vKCZ*-FZo3$xkWhSufW@ZMUp1IoNfITu#vixXD;XiO zCa;vaHI)zj9brEKy_mcH_|pnBGC1Sz-xB>kjs=mpTg$w@UYWbs!EM)~7z zpp;;IgGRl4;#8%*H-kbGgT^_YT(@7c)A&v#vsWu|y8eciy1^=kN$zakR0$8q8~nX- z%ZYMQ-b?3|9gw%5*M(@w+P!a}hhWGXMBSgo_bCiz^xyBB>Bqd>LBH|3ju>!%wYcG< z#IH?6?{@ETOQz%fAkgf+`7kE3bVBtf7aPzWi}LdaDR+6`l1`&o6$Wv6Wg5u*GmLkC zDm_&bNdQQ_@GS2NEIMNQ<@!q#PMmd0Oji;&oD-eh!@4l6lG9AiII?sNra^dz2w z!dbD)Sb`1)EPZde%OWZHhmT}}qNvy@EfggoCX$bZgBXy6?kz0-~lc6x3XBVUHYGq|e;D&R=vauIv&CZ4}dM%H%P-Sqg@XbAlS<9UM1 zA|QinF|^eDEVbvuWmM(1_ehUp1LqEoP$ zj5lEyqqm%E94F5a#8BsbKb9GPfR{L*u)tBpVI8HxK@1b`5cdLhmU>_4K5cuC@ojGX z3wsI!%d~l_l#yg<71gMoFM%YHS*yKXWc?q#ePvW!N!Kn;f<}N4NT3OB>85GH-3b!h zCD6FjSa1&-f(ExB!JR=bic1y?5Oo_pEjLoO)_k?W(<>UH$Jl z@2ey)A1_#N9&WRq;5&W~|2$b<6rGHf8>l}>`t3dvhq2SDI?dhyuX(`U68zYVQ?MI7 zR*iIwCtG|jHwny0wbw>;%^{GuV4EH~WAG?@6(kwoz>-8&dKL>|P_v~Gjj4^{mE0Vu z%nI+JOdA^=m`bS{mJI9f)fHC`(s6opGr57URb2+NJj>RKsi@8B{+K_Pe16*|;p(jt3)GTE04>;mX1Jl(M)C}d*tpKkE=bv5`lP$_0hlY4ewe)1-+cVk-J`%Bq?R(t86n5G7bRx3g;>#O z#$`Y>lgE|{DKaBI6HL>?-V_*C1lhI}*zA$takr7$_Rf9%bjU&-u9a-F1-)He>wMR< zxjC0*?><$T#`Dd&H9gs_B}K{lJ*=TY%yxS(4gNZ3nDc1KOx)l%P#9wy}f4s)iXsdmL`H)oCZS(Mq z+~TswwchysxYg|b4DP;7l9i+mq1jJbwQ+)@V9vT$e+LTjGaL^Z{$$cfo12x9fp z+GoMUw%{s16TOF@rJ5a|_HYK^ew7GcnjhXsY@nXY&ZJBh5z9Gz;V7vv(?*+|cl<+WO*2}?^h1GnTa811=?^E=KOw>kMT-Y-sBCOLX;R#vovX%5av{RPLmoe;x*~t@OK0`RkMv>VZ zyeg^e^O40Xnt>+51-_O}oGh5%(Z<_SG>i-LVVRSY+h*U@=!;5=?6o!;z)NqpR61a5 zuyOpK+@l?={agK<1htJy{Pw?Qr}90wWc|v9+CO$6?qj-KGF&d_`0^m5w^cX(FD z=wmEd&IZk4QeaZ+WEgPDx`WdMDW6qMTJ=hpd6dq4hKM!&A-={oE9z%lcO+)maauEq zOa0M^O;ZN}{^ijy{q=?9gA~-%xfZ~3W_Rcm-_bp`X(r;Px>8gxJ-VYbp-Mbh7{0}Q z%6F4%5wT~_)E}iT-NgEF?>FCcGeL98y0LH zKJ+d;sLt5S`&g%Yi<=?!;kkN2K+pYaHCc|6oT9U`hc#JS#>%Zvc+;OOW5rFepZm;x zRk7A;3}E5C+|qtBDx2eWYy3}1TjtRrrKaEC6{B%{FskLuFy6rLHkS#V4DlE z04D@$3}DHtq1DbGG$n5vLKU}>%s#7--nJ?~te9BBmpNb0&xHe&e)4mz43*_k%?xHT zTZhUX!L_hQh!MHZ{HJ^#~QwzHJFnQ=(yB6*OwT zYnDs0juX@dD<0(gU-)hh2(vmAU*T4;Ke&#ZK3TMKoBl~2weTZlSjkwi)HC5ZbW76O zZ%8qYzW;ooNgX2ZuR3<((giU5)NTJzR4V$wBZu_?sZM$Aw`$;Q$yRfI573sIHNO_l zYC5<2v-{@2*j!h;wq##?dMUN>Is~I(%xQe}J7u9~lRKrl>#+}|wo`KH?uw`@+1e~O zXv;s5u;bSaBi>v4y7+!9mmLvx=LVPZY{CKH0R6gK*q~`d$?IHEa1Xc}4D7amR*@fo zc1wOZ@1E%uy2ZoBx#3?#bn>8EJ@wMQfno5O-OD+`x(^OR?%*xI4~w4)LU+C<6gsn;>a*~mS8Ij6hm8P)^pHg8_!7xesqCCBH52GPM=rDoHjxNzuF zG^!caerUg-HMufa(t|D~=HO-E*Ujs~2dpGR?^QSgOAl^+qB%Nwp1ucWZ_*B5vZ(P4Sup6KVqZ^jtd9}*kAV2Zk0i+`s?cXdd@ruITfVnm6a^??xumh-Gp zrRD~w&|6T0q+;17;v+iiib#I+1r#DdgF(Rp9}t%`2tR2JtqnbBCgy&~ zhbK#P9v3_7^x1v#hWv1Q1U*u*UHbwzGDzgYMssr;J)t$#I*i`^a(N%y@u-4TU8Zo$ z{Og3i`A1hMhutHSYL+Xz?w_xp)sC*&1$6=Mb3em73;8^Ns#ZQDL^3^|jbB!_K3foh z|I@Jao|OvR<@?CH&0{kCS;2P?v)}yc%Y^U;OPL^H?Qbf?bIqhbD_#ioZM^beo>bZ< zi1Z*_XU_@g?X?@`nmVOe=W>x}szkG}z`P2n9^B;~IvuPojf)o!z-?(;^AldQAL=<^ zrZP=Z$?q#+9VN3=M2H(H!1}48&^r z-Ym*-qWh!rP0w*Km*u@$Ln?l zkGLqz%);HFh7_h$kPeR#v(^ogD0@`aC?ohGf78`QE47A3BX`O*|c#)#djXSLUyMLTTfEI1T@ zXEpfrPdd{^(1QHG5l=rnNwiRHsTit^-~p>Oh2yT4jJ{6df}D+y#MR9(QXcnIU>`9K zhah=*dt%##`FO>H_1ws}e$`cG(?zoHQfv~Oc#ej+^( zz*60fA1A^q5Wq5YEPO!B@VXKQUj-;;FsklM@;Szs8Fyc3hy~7UK1h6qQzomEua$1q zp4dhA6U#KDPWha%RPY!|g$Ca6YxmAflC)fn z&$43KdH1r16{)-;^CYAxJ`yH#Wn_n$8o%~zUB+p{EHp5>*M^f%>yxIw{>GCZ>x(xM z;s^~Jqdh>xD@))pcz$(&bQ=1;}D0Os9{ z-}pm-m*-t|CBTzW?=u{}Dc9MI*r(>5vCEv0C{2MmAL2FP#NK@pi`Tv!xd(X}gYzd* zBDi#X7p}sO7C9%kDx@MA26O>$_yr6{S*&yx4GV&OFsWv}9GoPGB32&SBwE1>zfpH0 zzc5A2&)2fn^ziIMlv_sY9Y&RDA$%z$@T0BQpAUoevR#|^9%fO+=v+vnKjO7b;r|3S zBbae1#hvf2d7{m4(-QNfqTo5ql)W>I}QJzo)gF&|H^&!o4T%(&+ab)1o8%AlfB@CsFnL%EN==!fWRX z9^t9&@r(HD?}xoN)*dD&mmep0WfQK|4Ve2cnN~T=`AII*7vy$s66|xtKCYxbmlQiH zlL#Pqsl|sKppiQJ9-C)uw956SD8}fj7|7EYt<+oR&^P?PKVbsn(xVOE9bDXJzW0b1lVk@GnWv`Kq7-i32ef#P%WYrl3 zN77p%Lv?}4;TeT+d*@Pg*M%03Z*RmJgW?gw8C4wsd|mWy=RmnT?CkA1){3TE0>-r+ z(UWerEGZ8W4{T|!++5@1P#lHr0yNiC8FsWHmZ#adA}XUiuLQn4c-1H`OXf$qu2I<> z90|jpt`3=08VcLdE@QdBXuk~hYbxQeEPL#v5Ps}IE(RM`vEC|4F1x>7=kT%~VJ^N2 zCEXQr2Ac`qS7YVN-o0|H=XJgQuzbI+Hf9^74aAgf|HwD&y5Zq6f-6p#KdGXNL zS2=k5zK%e({;6~R$EdWodGE*-nxbwF>m|eeD(1JqMyZ-WDFN<*b5F1DTsH<%{6-U{qRT;On-e&1c!-5rf=XFsv2n@y z%$?r2hBr!mdTSfpXcq2{-6-79<5+!NVxz{xQg5WXot?wfR1=M7EBTat<=@jsjM$*?5NMW8oR*0emoikLj7LdpeD-X(U z=iNMdD@pa1LSg0}d944F&Z`L=M#sr_Ek|#L7zYUAcoFW%lAydQ* zJ1$_9eaJoW{&6V863Yp{D<1F5vo$qO8iXd87K`iEFASDfC)2cRj^d0_BU#aE+Dsg3 zh-&ND$rNL7pwOHBZ<|VbB|WP>1!X1H<;E(eoO4VBkLu^5w6n^seT|hClD3)J@RAKM z9XsPZnyW~N^lResTBW>RXz^7oZ7n*v9}P6g+s@57Emkj*&wE;{UEwNtEzO6s*5_VX zXl-5NQ|OWxqf2_ec4IoF^`z<+68+LvP_2gHR7%_L9)zgSR03bex5egu%cf5)+!ZyNIczMnSs|_gMpWhBi_K4{^|N;|0n6)S2agn z23|T$*&Xcj4YT2g^0FMK5!WBDg z)OG3N$#(FKvGay)(=hJ^p)AHp3XgDRHhny~P|xOw>QmLX8ymzVTYcE@B$|HAx7fb) z-(|~>_-PYhzCrFP3GQV3wb?@g$>?+BC}AO0Ofis3qw(`?BT(Lw)%;9);4YR>npf zrHf!sG@5YT`gGy`nc&sa>XcL_5r3;LCd&xE$YRxRQfl=B85(od;iVK+yoR2#jpL>5 zoWt7dH8b|zmwCH7^OVx+T-WHg^2}8hq590VJ77u?6!=n{U#gj> zg7bsVhBcFCypL#!g#=FQfM7w6-qV;xRmBpYs>NM}k{BMzqOHtbJ$*eKtOFce1(0B5 z2?wTqu84_bGfzbg5EED8F@;p~i;DReA6g~)ZMkOfK~NDq-JCT^d$}!o=osT4Ll%bB(-6%e%0(Hd>%`L#}VzS z0Qw<+BaVahjOm03k3$Kp2>K!KLXU|~pv;JgPB5zZv|z|qBanSUsKe-ak6zfQZnqw} z?-P((jzM`9Xacij_$}0Twm`-%>1jc#E9tiWu^ih(e~No-v)~wAQ1z>^RaUIPr%A1v zFU?bvtRMRJgPl=UM9AClVkgfOL9TJEzTY&;_!65fURI&5-#1JH|FG^}z)J8Y$)sbT z_sOb3Q{dLX?hYFZ^~;BOna-gc_Z74Wd|^jp^?mQ1Ejatax#4-?8&w&3nBW#P0xs0^ z5Cz^+-ZN1zY62C&mp^TB!rvMnIuo4MFF7zt%~#lk59qDe5aFZdqP#T|!~J1}J7X<~ zo8Ixw!~OazeShW0dxyKfej34|uq1XY#=-_#>|o(p^5XM*p6}VFws6J_4r=0UM)6dM z#IX_xbn#aw4q}JBVJ+4((8d^# z(l5e&>bNnB8v2se#KY`m)I@Tdyzu7=9Ae>FqLXZxV@~{n4cBu1WN0K3GyqV-)KU#F zo3y|Sn0AmaGD?bSeI1E~JE=CUu8q4KR?rYrZ>>$4jhAa`U|=ShL9_gNI zNLt_tJ3cRX4-t*cTCK^jChDReS~A%huZpqNqz-jPbrpX^saj&I7<#qENmFb(jk#Af zibox0!Fv7R@lg;0kIC!AUtWPADV6NB@uzFsua=qqejzjZ2q z(ZaQfh;d|+G`Fj60?_nVR}D<4kJw_B)nfG8I%&X$Vl+{Kc_nbm|4rrdsX`;Up~0e! zrBy@ziWRWQe&hZ?6Mbme&`^4NXq!xXYb-JA&Xz`E=m%EILRL%sojz7(EHdg?R`zTt zP-CW{`{P(e-e*f0#igc)mVNj=*-%!E`hs-U>k*KO=_j+mJ~J~-%zkoT>{tRe96A`# z#9V!}tfW4oa?>+P`Bl;fw_2B&LA{Ds5`Z*I$WpV(36sp*7775powe*1DCTy)Rq6{&BF_M_;yFGZS^o9y0@%7A}M1JUY^cy?XjR z!H${rZ6+2vqqTTeO=+QHRYtj5C{77!7HWtJSS72qRgQ3KW4Mgr(>i<19W<)}z9}_J zb4}@`P0Sr+l`2bs_mL}lbRRPbi{|V-47MR(6ZKMygpmZI9cMDJVZFLO{Pbqzhgk)K@+QmKD>k$U7AeO8xe;x?9nXZcA=wCKY);k)1-ES zS0}o9b7O3vI7PT#G`2T+hI#)YL;nTEx-rMQqHk|(JEF0weV!suv zW!4BsK`x9S@xw1Hx#LvxD4)@XUP=T4o(Ix=6DXU|MpwZ7lpG;Dz^z61WXizr_~CgH zWlB~`Rq|m*F%>PqmI2S@{->^pRl!=!Az6tR=HliW0820Ay&*=&j^#R6E9Ssb*VCEK z;CkuUn9`nr<&_yO$C}fjDML~o5R z1s;&(=C^90@UDl39i;w)*IHPcej*%f$_94d>uUzfk^?y?^uujPO`gd^WTz zu>8Mr&Zlgy+Q83;b*M=iJVh8?9yu~FJe9-?!yz1NAy)fOmS44 z+Bw-dF|)2W7`I!sOWL*Un~5LhPikkeK!w0EjGws6Ph9Pj7h3OOb7FkK71lxfj5TB^ zaR``2lcWih_@2^U@|eqopq`1^qz2o)xm$eD7YvT`>sy!56Yw<})PI74F)3?c9Q|?d zxl=Q3Uj}?0>p@?}xFz~Kqd{gknn?M285CuZW$FtCAVGAk_ZIGANAW30oH@0y^7vwR zpkNVN62*)nt%?#`gume)T=dGsaG8euvfL!T zygHZ8h1W(+z2{v>7jU8Z*YL#CqY7IQRA-}|GV^!M@|$Y=6XxR+X62uh-i^?Nm)MRw zjPMG0y#Q7|@69wi?hj%&UP^htBZ)^erK59zVjFvNRovz$b7@_$7x&1e zTO98V%gT9$(S)6`cMjCI=WsWUEP4*w+?~~LDHSUP+etI!4m~bBxu4ZC?er~Ice=qK zfrh9q86P2T6i{53nL4O6b5hgsK?rz6eU(zGwm2{LiZ3WxwiR-ws)k~o%9rg)a^(It zxSM6i)o6X`8`=~vd#x!MOq=9c(4gWEy^bUtS#P+}vc#((t{L}0>2JiVSTqq(dG>S$&)JPQ>dMW^Y;5CT4+ zdmsk9XL$Y;yzqjX*HGN>V89s9p7dAd%Oy{Y&=b%18al)4I_(9qrOs-!uXDYX&Yo0{ z3%i-Kl9(&WROhMXm8ocp*J~^pWn>?k3B8AcSJ-C|NaIIueWP)uv&1-6hZEzqvLn<+ z`|203lrH?Wxa+kP4~;PrJMoXDEDyJOK2Bu728s8A6{T>GC+EK}>$db+T3*+al(Q z_@F?-b@Xv!ltY^raupW=m4ZV4gvhmNAM{tp_*ynOqXoaXnpA((C_mrYA-h`4XfB7V zNa3qvnPUQjba$q(uy7pLI@uaf+w)D1oZkYbiqiYm?3+F3f8nc!teI4OYT-=?8R9B^ zMfjdf`cWMbOHI}nf*<8X4ZLb}T4|zCGY5k&!#~Hk2Jh+Et8S88^=pUJ<|lsuwQjzY z+-+(bddsX5=(S8qrOrhAKd95)X|vtR)3I@Kfc~R7%=S;wVKHkfqzJ;^z|PbLX>E7c zl-0LH0EGl^FJ?hIQ+*2=C7jzYqZr!*AUJmxcap@cFc<{D$_|DAz#tF^zy<=rw1JXH zeG5|qK`Ubm1OS8s6tp)$SRnzNASe#-*2`^!70kwl0~FS`kwBOln;>roLUDkKNQ9*d zfCKc012Izz1Q>89Ij8V^m5r zxR~(ItUYg>$~yiD#!J0fZpjTU$Wz)P{M>KPTQ@?VDm^+|CDSqMTrW-A==+_!RKXE7b$ zHADQWW%?3>oo8-xFPjN_dA#BbyLcj3)R_;k>PTRiXF=}IoLlgC9GcyaMvkiY29K)C z1eGM*vErWcwVCKsAtOfR_4%rCe(OfI;)hUcXMZcZq8&T)t5gV?X1&){9- zHx#CO2Jr>l5bRp5ThqvSV#C?brCG0~kKj>_m~uDYj^NUuYw3k9U9*i{!_*ssn}8dV zU8{9nyWH%JW@r7I#ts!|9O-apx7>^JV8$JvU+3=wJOpC2bd?$>k^d8gVgHdPyc1Kr zoyW9KYv*LSZ=Ce-z7q$`^1~-U>KC zC2M6X)7!5Y0q!d1zia4k^4wxKh}3|9D9L@etDfufqQa73B4e5kUD{!E+>l?Y2_^sDQAycCa%**aPl1 zpa7IX7@F$eil^VMfdc{tKslk>|5u{vlD|dMfx^~SNQ4#A9&jsudAs&M1>OHi&Hvx) zxy|9<*TeQV;Zb302P-5147}q%0Q^S=Zut)Y-$m;0alPxk{hQ$7`8^H(v*@d7A| zaQ@Tt-vW5Il>ZmU;6EMTra<9;=UCnj;rM&|e{+1tmfw!=&X3#F+2j0+J~;p358!VE zy4Bs6HZ`=@1l(;LaJPzE@ORYz<_Y{a#nd}b!um*k3v1&)JpQIF4p7bpVI^pQG_|(+ zBbI=>Jy|(eSp4w=(#8SGIUp@et!}A(7t{aB;Gf?XxXqv)LLX^u_d9nG2nYlRgFrAg z5C{(E!?~?HLk9!IU;TgP75v){3IajIKp+_G_kEl5+x-8Ta|j3o7X6+3J6ERGRwDXH z1cL|{7{ta7;$Y(hgTPQWC^HC54+7CM{w)~Jb_gRJ5C9Cp0sVCWU{EMK6kr7S0|UcB z9PEI*UVzna3fMJ}Vf6WKu zWdGNEaCZ1VV+Dpo{uLK69Qv=gfZ_0euMf@v|65#;cKW6k2)kQ8Dw@7U0KgESg0(dg za90irI6z4&BWu7NYi}DO08Ic7lpVqW2Z7n493W8$Oc*XGCJf^ggg}HqV!|9CI0qlj z|JmfW21Tq5giR0z=JpPj0H83ZC_6g@BqRtHfeC^kqMRIHkSGiWXXoGm!Qmoow?$-+ W)VD+4Q4`Dof Date: Sat, 26 Apr 2014 05:49:40 -0700 Subject: [PATCH 062/320] Added switch priority flowchart --- Documentation/rc_mode_switch.odg | Bin 19757 -> 20160 bytes Documentation/rc_mode_switch.pdf | Bin 30076 -> 39286 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 70dc503c18eb76c1a4ca5feeb44149e8118b980d..5c4b617e609a916e1b88893f3e72c31fbc152f5e 100644 GIT binary patch delta 15127 zcmaKz1yCJb)1dFYxVyUrcXxMp0wlrRT{93MKyY_=f&_PWf&_O-aCf(@yzjre|F^rf z=c$>l>YkpSIW;wNx@)>J9o(G-MpjdVg2o1c;6b2T^EhNxNUeE-F_fODMoEbO$QN^BMM^GlSw2_f)8JU9gsjGR zgC(dyGYghjFal44t@tk#d`UaDKq7YXEK6K{CvXBXRA4_LhYlLSJ4w0a-~nIbc5%e*aStXdF6OacWFguwluicKdeN(}ctzQ9si4>mT{A*MoGZ$E8g#w2G{>;M{{ z<&Nki#hvd$f!~$hu0#=85RA;t2TUe1cpU}w6Ml<@VmPW#cD?_^j);ob4Om?R!@HsP ziDdm^Dgydls^oaRw?BV}fwZU^9j;{}b2NTlsKE}-FefWGXQw1>u(u|BVZ2zy<>wF- zY-@0DU&iLEJ|rN$uIgqXSY!%NSLz~aAg(t1^*LGkvW{Nj&!$&oN_x0P*~d1xhhl2W zB@M$rrYyP~nTv}Ei|;_b%D_fg6Nj30;$e45F0}_q*<313V5&b8P{WcSN~*<@xHyNf zNCh&Qo8+I4lX=9$1b>Djn3?&yE}sQN>TuY|gMNd8YuN{Nza)z5Au6fU0dJlRwB zfe81~@hO8EC2he6B@y*-soi0*QaZ&dU8eSXD|oYkM+7EMoKuP2-+*OTVu|X@`3p7P z!pTlhHE%Re@snN`>I3Bhml59pdp2Icg2%oGQuJO@CWO4Fzr!0O->&@m?fSk~-KzmF zD5pQPf1kD`3J*qaTr}8gq9L6;EZP@hgya}rsW}m!<9hs_+dKU^wkFngP7Ixj2R;B< zjw$+!%$`BsX6OjGA8>tk#}Z23#MZ69ccj)W+ywHr^!5-xhaxO={K0mA9O z!-7hw*6G3hWKWb*cwcxW3r^)YN8~rYwupT972k401LmeaY-!jq?GakXING5!f^z>b zOsRNyJBbGC*e1F-R!FI%AxwfOgukZImQM{7D%Np`b@ujj(Dkyg)}=J|?ykurejsWk zTo}PfRFG4Z2{67L9GpHS(R;?H-dsd@EHmJAMYDPoSX~lok>hY>@>MH#(GsUWL|syz zSihHQ#m12@Q{7D}7&*NC_Hnc8nCZw{tk2_ zJhHWSG@f}`QLuh~nn$(A{Dl(e)fx&mzo|K@>(>VH9pHFC+rN7o18B51DJOILl8Mf^3Cf2*)|L-CcQ}Gr0xj;o4UqY{l{CUVo0Re4C`+NxVB(tC z!<2h_>``%Q>1v#gjq&CzMiqfdc0enqLr}LWmO9z)$l}S1bfC5k=xc!#Fdps5uanGUt{|iN6ic%&OP|a5{Yr8 zt_nGE9}gY+EMXFlL>b|?*<@ve{aU@cqi(ZbJW&bZz z9yjcnvq2B``=R|IqKv-$(nTV?(za(~WT2Og$fo+hld~TG*Z!I%HJI=3v(5x7Ky+At z;K`|mY<1oDJB>B=g5kBe1}=nq_|qHBM?q&Z{+8N=xP+Yb5VX$EzA}aC0kdkNHd49I zI^HgL`B8sn?&jZllGRw=gq+9dTq#864pBS%+{j&aVp8qTqIEqjWK7DZ9TBuT0UwbV z>n{nGUXCs={hJ?mGPvh!@mG0Y9DKfkqt*?W5V|XhQb<3A4#j|?JEC_!wx_ML0u3sC znQEFk(V{>H5?hfpbb4m`SF%nYLP4IEC!B6>tEFo=GUiP7et3ban>|^gvrpMRGy|hH zX_$tl)@tEVette0D%pIHk))`0;1>R~(|AZkpef4|b!qi z4rO_&;t&J9=sr#xM{>kd5T$S@IKsT;5yj}lw7Y(R{XB!UkUU&nXPa_xdku{O!#kec zG5-zYdJ8GY;T^{z3-flT#xUkumQJTbYg@VSf(C4{zp^AvTbfEqdzGw_(;S}FVp8W` z@_PFR!djDLM1lgCe(M3W zvgzzJ)obr=vlv5TnDKGc=dny1$TzAgbjb+gei@U!6{Pdf7Gs4F=#M}AdVLrAY=HbQWLJnD}Z-0gG7S8Ip$8d zTd%dpjq8d;#?fQ=bOK!P1$EFD92rHVlw+|@=`R3>FN1a~3**i(JgUZx;9*D(1@;Ic zSlfRx>N6VYEX_5_8Go?3?ehiS&!ZIY-870c)@0?uDtgGkaGQi(Ez#9ntX`ClX;rN< z+Wr>j+;Lg@F~#YS35cG51ih9Nq@_rO?<~Gd&poV8TwYE@jRmoeju2g*SQ&q1rA??4 z6f{ssRn3O2im#=PJFV)@vMd`YRTBN4V!`xn`vE%pWUljGf>DZ9lTn#oK|ZnQiudh6 z$Ltl%l$ahdpEAH7nA!n38ZTCo?AMm}%?p7uzf|Z}Q(q%@0j!-~+fl^9srf@EhjU|R zXEZvE8CV-XIMwsqg2F$33#=`T(if#j{zchUa{xmpiJUF0s3dL9%(fp$PxNh>)!!9v z(=-oSiAz6YugSy1T#du3sOAw5=RS=;|J!ubd~I`fR#22PMvvR=OSp1~^4l6^&SzGwV zLFwbBbS?IarF z+NYz?4j&4S1#!A^7`f&pq?{t1qNx=6IWyG?1$JLH5uz_$Q#pOD8i$QA`mB_OTLg=% z4X6Hge>M4bf8{CasXZ<8xIcvcRws;Eib={fpDp9^)Bq(0CWhr4>ilR*?NQUxv6v)) z4JVz}09;d0pLdFh@3EQSBfR0i=sh|XfRt+12Nzjj-|+q6w)R@TKz^j<4Qv_|kU6S> zP&xj-60?IKP6&p`W~^J=NZ10i^yL>ss&E+C0rXY{#vqtYcGDJOO0ir}yMjAD~R9`y9UI&tYqcHT7sDX-+7X}ycg%P?{gFZamP@ zYo%T^s;0;_L%JXkoC_lULkX3UpJ{||pMy2MBv+_pDpkZdDpLc`nnoiY&cKbtx-gSu z7h-Jav&57i2yyzRmG4oMt^cL9mC<5vbX2COLA*7*NJpz|X-s<4>ku2x@9Yn{r5Kp6 z*N~;;nR&A8X_-Zt-gftsh+I%*9Y_J@8EI+cpRFA7V3frxzxKDi2_3jps+651^cVPbb1Y0aFGE)mff z^PD!(sD|F!jbQa7^5XdDSX-%qbPj;SN<;ilI~UsK72D3Ga57->JXn~3V1V%7*_JC6 z2P4b)zy5k*n{9w2*SUngD4Re}>M0R(D41+_%X#b;roPb5{*74UuL2A63(YWPpZA)d z!0oGRW^~=Ee$}F@xoN&cleq{_PAm=W3wTZg@!!R}HF>Ju|^XRSlGE^=~e4tG_h-3$$mG-nlZ7Tc*7H`AtY zz!tuWbL1Y3dkxFvQB*rQywAs1BZCoitaRnP1|SyYf`ktEZ<$t&Mi-I361{n6TOI4jd~{U*^&Q8jP{9F*I%U(qoB~bdLzK1KHyO`Wl?p~7N+vh zx&EzlJ5Khb*e-B0G@J(@Xj}xHr)8EOzAPk9EfF6aMOnANUU;^|A6vIKSPHzD7j?(F zF6=T&8>coiZ}=}h62l6@7_I$zGV1dN1(7G22%;Wx2j61t?4P{z z+`NST(5aDbkT^-84f2QOX#|Ys`So!70%SKWWE}hNKaiMGs4P0LE;cioQwfnwXrY9! z=9^$+)gY0pnL+I}-Y;=>F@D;#Ga@m_1+T^0y>a#;RAN6dOw8q-Km-mz4PpXG{Z8l z(A@eT!aX3%I>cIKkCf@`@@0Pez+6fn@^>v6FK7GoFi{hQhc+pHtymU(ElC@#7Fdbr zae1Me!nxP_-M$^h&1ZMOgZ;jLU+{bqI+>_b?9tot2>b_64Df?r4Jn8k3rEZ8lO_5z zdPOU8{9Hl3UJPu1uQs(UAzaAmgkx&|_Ihy=0g?Bo8Q7$~$n#6teK@!vq)R&!eSY3& zXATjzr8_!(MkwO6RxB*n^Z^BEo~a+b>Cm|?_$9h>UDK25BfoMzUJOp7mtK7v%qM?) zMs0ulGajRm4%o~y#OTC~ZNLeb@XHCcd8ECjWv6P~l`PdQlyWEJB_@kc`kmA^I-j}} zw`~{dUt$V#DSv)t!RyqXUw)1G#B7m^|7*Wv=NE5}g>F5!l0eh-?nyif&XEjbTK)A# zBW^0(PZOW)R-+fBts%J>ak%gv9v@UkNYvw}SoL|YXF#xLpfQ|nWq|Fo%m!SX56ZSU zbTm-{45N4thHhBi06If~BNa;0v4ud^Wh-f3pR;9ipb32SRFim+ZOu(0^T#LS?wC$7 zD1`YBxfzq~{5JQQ!SiPV+00=Vm1|Uay+2FQU(Cbr6BT~efhze7gr2ObaWt1 zVv@qBBEZrdT^#HMYtxOE3u=cZgYrWCx85X+-vw{8g^1WqpFUpxNI2p)fG@&q(4xyi zv`(GIZZ?daCX};m)}X5#|IBCbhpQ94TklcgTcxh-{7P>L`@BIBH|ltTs?=4Oz%{o_o}~~z4*?BM4Qd}z|eZFQAYCAFF zKVNJN5W?K_@FWW z_ksnpbgS(TbhTG&1VGaWIfAUI32#VmsqY;Ilb`T;qjihf*MaFAQ%Y9otFR#t7tQOV7t_%)EGeu^$HHTtqXVO?Zm=95Dj;FlBMx>F@^5RY9h1)O zO4Kc6>>_{n%PbpDpC9IgZc_a=IL4m#iCR-@Pa5KiiDTB^w&N37U$CB<-*`W9fE*!m>WNAU z#ac@w<_6H@@36vMW5c2EXF~Ui7=@ONx9`_?Kvd4;i|eyF{Of;Q&fPEX^4Ra4ZX87) zu39)7SS?=N8tOZ|_-=rX)Ms2dTh>-NMOL`)zuL|#hr?NfY^`PSF0Omg>{uP$d*(ak zPLn={|H1Rz+m$zObhi^7YU3$(Z)u!=IrUUwsH?QhyQ#M|)lZ>IvKP7Yw0|aU>1$D~ z??kRxq?f-q7FZ`s2@2&yWsxZ;DWSclrV=%b{9&D5P*{)dr9TTG>Evb1rYp-f+RkK1 zQ$?QBRkzif+Uh=7ebAwHY-n%l%hEO-&?(Xl`V}pQbJ;x0=J`a~h+_FECL*iy$XUH$ z_aF%8)Xq}gd|hjM>9m+~asnijEQ4=FRpd30$03{ycYp_RVI zM8908fQM98P-d72EFd?^%JB+Jcy{&y0~nQ>$QnZ)u2OX677TScP=aoyjZ7(oFHQs) zP!v!-Xe_Do)}rRj1gI&ny8Ol^P+WvKcoo{mGas{2vHGK`J}boWKF}8eniMQ>bs)4_ zB-aSNMI@jl(PfeC2kxzr)`lB~PCAPOqnA_h!%jEi0T<;1n-Y=wQ_lO}cCmotycvG% zP-EX>4OL(+XUm8EH)FJ%Oaf_XjPOx6ej~-=&LgbThltc?Ui8vgXvsd&h`ngViis;>!8S2V%x#K_-$7&oz#~xq{cG|OlN{Q!DbB#ebCKaioJhah z2m1qb#fh%y+n-JtkE(reEANG3#d(`n!xAU_^D79lVo*#)b2W^mi%i&I?miQ1)~{&o zE?@C^<92AF7dMS4$T>!*p03U}O~3jdZM^km52~s>Z&sU>k1n`hb_#p*H=~9ttlT<% z7`g$fZMXZ7+oVXWQ_#_Wvl>Q5{GoU*Cmq3|<@TlkI{AZOI}f_*I#r{T9;L zb-($Jj{q~)I)C8iXTwJ%P1T=cuDC+%m_+nADIss|zIx$4PW!`7f6fS0{HSw|30{se z%=~=odmZB+P{<4LV)IA>Wfw1yU|mdL^T~i7F%4;6cbg(j21B+SUQfI^HG9_n-{s5F zX@Q02QbmeGjD)cwvChRdKEk}kFK%{zmhJZGGuBhO-7A9n;SX8w5wceNyXx8=gle~b zyJ|3uau2Y&4^%g>GeK4zgP!KqV$iTz1x;b7)RXVDE*T-Zv?xi5Dqw~fUn4r$)DfA-UPVEP%~pyFm3tx;o)JTZr%D|PhZ zo-@V$IYU?ym$hn}fEsjuPuWr40Lk!(Lu&1fD#kPgNzbEnCMg4ixX~V;nqn0LRvyk} zxiu^K#dCl-Oi#yUMI!7EA|vb+8V*3&bv9f4Uc`5`DL75Wjl=9DJsig594f#4N5DhA#Q-XL(YT@8X#Y zs{QZ1IR)KDpt(+tOuQ<(;&_Pl<4nE%uG}e<<6TeoD!e?obu(M*w5|e^^qhgk3EA*z z9vTNGLq_#w#c-UnGrz<|zRfzZg+T^_J~f~6vN(1Vm!a)5sxr`&cqy#%!MR z1JcS=Dvz2j9Ll+tun;_oQ{V*sH?8r9^Wy7uhaLQ7a#!^i)v^b(Aj= z-Z3+9A)_9$=u;AL6)C99Ft#o@pxB}ASAzQ~4J9-CPt%o5$#VG^{9`dtxdj8EO%gNO zaB~)lvC;?!->Tvbq3kG^=e4hwZ3|JkB-luuUULEM-OGi9dPjAwsFjhtF0iP=zg1N< zT(jsDJARWs;-W1gR=rrYz5hD#GU|S!Cx>+4e)$q~ZK}^zC~Nrv_4VQHO?N-+0aHd( zo+#aLTmP=GP>y|i8oB^jn-z?%bb79Bzgsf|e-ERKljQ|hIDeIQHAyTlJ(v%$S8WX? zEyVQ+gV-u`&SpR5WikGK=A!m2rid)8n~j~$`x;p{8@rtLmAWvq=xhB17j;h2=j+pJ z*J3_bT6j4LgMIrqZz-Y zGHIh!w>Hzl{}LI|;m;~P?dP&;+IYnB?Szego1O8=YO*p(F)O*S*9`}Q|Lar`&f*-a z0xG~XebmzLmtK{f*gTIUOq%+9sXKe)WoT%&Mp3k@R9(T#mG)3XUHJ9-nQATPJ9SZ- zG0gkBx;rL63=?eNV}qZ>XxFMz=jR6+O`*zUPl;IJniJt)eeW&%e_p0|5O-2Hm6Sxj z9?y^rexZ}o!>Y?!l7!WH79$m{4`(F2k4~|Z8O*OK>P7lQ@qrlCp0SnKFZE|{Xp6*a z*>+Cyf)~OtXRwI#U1*+g6tknco$LEYPRr9p3hPJ|Nts#)VE)d z2a_zJQOE6#&=!i-cU{WA@3ws&1DduSGeu7vmmKvE8t+%+2#v}U-!MLpL=C=fXuC8s z*+yKMphb|e=vGO~!jq8Spg6VkbVc-{#cg#wvC#tUNSPQs{dZ?gK)BW`CS~P$P$ai^ zp>1trM~$HapyWC6ycgO+ZRc2$xH9nhha_jiD$^2;fSp5W5>c_QhNo4Tqn||n{>gZx zGTP$eXaz=H4Mo+WN;yFzMa^kihPc+_9osn-t*}O>mf_j$xIzIfA#bhaYH~>h6Se72 zQV}$SAikeCYpVvD4#Fin2Xe(;T?CSs5L^|`9_vj90IOD^Vffoo+zkQNd&FUCGGr}3 zzvR-ACRiO7%ihH|Sr4jRM|9CfTyex*`avhc{1@EXV_!L_%}iEFExJ@hp!y;GkbVHte+x1Z zL^G$8Gk;ei&nL;XY8e|N4V%t9&nd3syB=uOp z5E%MIeNz(tfMmFFw+>A7v^wLQPq+vLlE*O@w&JWaANb0ci26OK$A+{Et~svi^= zGUVjY^#LpxXg98QGU z`*qc4I?lqbrx5BL3EM${{LqN1fWYJ(N1Xby1ZDbj4GE=2%L{>bRKY1FI|i+JBM=vY zqhV^kYFzMB95RScg{Am3vF?^w@6Eiop*QvOPQYDI<(2B=CvTN2l80wm1nVJE^hK$7 z3Rf4*NpZ`8ibCr@gIU@`7@7-Fl`f6=?I(V$IN==Yp4aiTv*WmZck`Ymg4pXY^9;2i zMOe~q?v_shBHLccwu4vT)$cr6MF7>5+E4!w44#&Ht}jCsnl&N=2k!pTyI3_?k2Pd6 zcwcaaRNC{8+-lsQJChCN-_Vd2s^`mBwIQHZqDgHEgU$yDg!w}ti z7W7yLcReUVE-6{v+>9UoZR{>E|K|tyFYCQSm6E)MUB>m`b!sU3+Wy{*NG-s@LZh6* zJl2XBv>7@0N=e$-4BkMd!eD#R=P-!cA4_|TuEAO96_g-qEzL-0vTiN2ItwX`&PbFO z(AbC8AaAGF@O*taYMsM3UuC_& z%^VWcWL<)Sh2^li5mPRUDS;WQE<~6X2s-K)r&yTZxvubV9WV0u=CSkYKgZ1Y;GoC} z$qC6HeFOLVN?emDmc?N|b=|KZ?uyDQN-eGtK*(*=ZS1S$cSfJ-JJC9(t@OfF;V z*Aq3XUt(S-*z4k8c#bazSM~XHcg+1eZIK{s2dxS}zBN@Ub^@oi^n%pG$d+y({=Zxs zVQ##XGyI7L={}f$3jd_3ZBSh{U#B=_&y0$}j{a0jLSQU!<4s=9*L>OGyMbsa2Kkpg zIfy{c`vOYkcs)Y4`IdcaoAIf-Ek?|51~iFrqvF7jR)h57TA9M!V!LwX2?d2z>33^%dRT*{mAvFARj2e){Ob@g+<$0&!x1#`}BZI}Q~lx?qEBR4G%5PBH zwgFdvXgB>519xB#eEMyQTvZ{vkv2617Yp+bC*WaHAyO>??>u6(X`;!m`28o2uu^2 z&!OvNtS3D*mazu6OJyPPrpY-RO^MwiIpI0sIq5n!A1U`e1kWY9{F1XpszIL#O%=)I z%)JUPxKgpa*puU$Ev_xe^K@ESOa&DmU1;XJQQPTeD-|# zxo~6v(gQ$xAU)8cCP=RV*El2Lqti~*mt{f?tT2YNb#+2?9qRx+YbSSMQh-p^K?E}V zFWEf9xYAF#6(gB<2GZ|OcgU(?mZ}pbK|lUQ7_=y9hFxx_Aal^qRK>@i)_8y&KS5In z7eQAMrXVjB$Iry>U7#`J%;HD_jOh@>PZJ6U?&>K%>N^|J|3ew+kJ%$|;}U|^`X{Y# z7EP=7@_xY=<2T!T87m0nO#g6&MtDH$goE4wTCvfC=6 zV-k>@Mj;xPtL84SV8o1b*gO>KXS=6bb01?VWt8hbh<|+SynDUk*_zI1GMeN2Tre#x zV{3mbFcH}o%{f*o^GEG>Lm$&xk{WXTF%@zH6-vWt*}&$8EH$SuVG(t7Z^ura_s%6s z5BXA{waYulY;@ez&;f|ZRjKueF&>i~o;!fVG%Nza&Ez{F%wOl5#y&lXF?;m}%~vA8 zamCb21{Ah_&DqvNCn#HS;$S>uRdbngrpIc;<9led7(e|p8|OW5F^+1YbmhlpJB^rA z7o35*<=yw0>fW@LLLTL&bY*>gVHdZT(DU8dnVn^ZP{2dXz_ao@n($C=`mqFAJXb)z zwOwCN>k?dXpyfXruPCny&1~M6{$V_+@KLC8f>H@FCPyI$q7(|xZCh-3DB@6VP##CR zw~X!dc}1Q?R4^CKZTCvRp&`I6reF2Ac@9!>*K+42n~+_K);-c}y+gzs06v+U*Fs^7UO??tcI;{$BQ=#VP!+x;Z~P^+ru z9c}oA8%TZ51je3)LRpsJ@JTK2=1wMgr3p7^J#a1VYDgZI{7=_uT}9WC+cr-9CquNE zC%Gama~!`2}o;kEmXo^DVcue4ksBGS*+SfQ>iiXUX zwrF)U4bjS9=5rCLDMvE0Aoi}=yLTL9NmB^x;2u3qDd+R=AOGTQtleT0NJcq&a+puj z*;wdkDQHVs-tm;34K|qH+wEI!F`%GX_gm_pd}r7BT*G5=bHhFcyo3$RhPrcR%cNQQGCr$aru;-3#{IEJWZjp26wL4+LM!R{cq9HWxK{HlDs%g6itqbccXl#*`SWG{k14s> z%nm!@llj7mI_FPK06rvxm5EKIJ~;jBM_z*!)6k9t>e;5P3|4Rb`@J8!)70zS7f4sM z85XoIXBK^n*%=54mq+d!PGpOkU5+I)Y!fWB!Tut_HGo<_E89%L)wriCO~_z`A_4V} zuPmHtVCLFr65rWLbmo(tnEX2?<5m5W*=$tfqN%=9eqo3iKqzBuxA#h(r|8fKXElt0 z)r6L8gCfr7&U0q|p;y`#5L#4}^J{xP8GCzCfzV9vjHj#=9L(&^xDkLPc5ic}GeY~b z9@b;DTVB{1nZ^0LyIc|AY~&g9MtpH5(L4%rKl}9f_5FBcEd0GDMLtFbOH>`o_yTk^!H`roE%y*~8y@bd6huIn7&`$9tIF}!E!hIUA?_!&j?2cmieQqhqx=c;Mm;)7I%$vJq8-tZ+iK|;OxC2)>FYZu`@8k zvdQFE6MM$9{vQrBFbP0_?RQrbZSSi-YAdTK^dj z<~J0y^S-5}Jn^G{`2q~p6>xF#=dl^)WnUe4k7k=hLzOWy>1C=M@0(0;+W_anEba#i@f>Tbu3f_{y(`lEk z)8&(`zhs+|#GlGYxlqzv4J{Kp$z?@m>vpU5aa&s(7X=)BWt_%|uk1ds!X62X zrqU3&$qqQ;@|2NsqIw*Mey*3&x4YOkkw!F3VyXcaD_Y`Y@>CDX>whR9w6XtgAH!fi zR<}QumCA_ptr51Vh&5sAz_56fk5(|Xg!QJr+*PS4BiH5pb>y{rAl}xqOCjM<=6Ga1 z&wQv+X~x5L6mJxh4ile5xaluKKy)RKATX+oactI_^>OoPpWqTZVFkj^X>}G+F^x{MyVJznmgHUbh?7uC z8R_)+hzq(rkA;$o#TgW$qT+9#bC#sR>r9=tgzj3G_V%HbdbFIrg+>6AdKi>V6mu!N z&{~-y_Z>~~#we^W^Qz+q@Cdg}8J8aLGYOs@_MOf-`W?od=wd^BX#Zz}^p*f5J#{mt zNqm+HA?*>dk|m#UW(tSoq_vxq<|;d8UL;`~#K!z_=A;UvHC)og74VAV&D^^J*sE}4 zkFWP=GXpb1uv@Y~*MiFT?f+Px~0<>TQ7f z8p=oT08@#qvPK<_NoiwKPKuyWCl#8fX(#{~@@sF8$}1Mm zhC=10NTCaXIRa^FRC2~)<5oD^Ubm-4BI_YF5Zeht5lDVv=Ljp_A{fA*Z%4Yi(evvX zCF}JccAbpD>gE`n%oR=my{mFv9K+Wf?d)iXid~Ila!@oyZBYV|-HOpBCz$Y9z~jR= z{!Xp4?(>T=Ee#mp4toFsIX|q3TD;DX58{7{Me!bk6$}74tyFnV^pK6(wa%X@L|KKR zsq|07e!P|&w(Rn=R$>@QZwF3t>rI0DS{K(RLSmtM))|rRAhh~>*=Y?%7zg^7ZaV{_2_xAUk3Y)ve3@~kfA2Su`9EI4i)Ag}1NfP3O zsbEv48H$?k_Oz4ZO^_FJgvn!>)Lkyjdv(7wWSb$R#;&OCWPd5BnVIKAKDwzp#Y z6m4WL^LV7M2HF6qg61?JMm`{0myzR(Y1IRaE#(&|?<{=uA?>Iy+QV@_64)0SG;!d# zLb>^xKtoI+*Ntu=&$$Sj3%YHHN5qQUI+At^27VhTx2=*cs=W|<*L5vIHX{9Ij32AE zo`qJFJcr8)EabPiwtj7#0@Hp(jxsKllS3G_CSKsJ&I{nhS(eqW0k*|Aib&3fD@3Nj zLOWzG%TL+yM{O_j*Opw-*~(%i0fAk;5!+y8Q6`(fI!!W;)rBD`r(hJBxL(q(k@x6G z8sjTQA3q?z`&ETBF0ze><+2_7-1hx7BF?R(cMsiVTJ4bX;YH*^_DQ8vwBgrJk%ncs zs}A^;uD5HTrRR~@q;>twQGMiY>Tq)WOwK3k?&8TV^`JBnv9Svx6yl@X9BiT6I=wV2 z9Un9+=M%^gINo5278LK=S$rY)R%IV^E(RI>PeUiDB_4)n4m()3QNOK4_!J-j-}1JU_QI8BCvr$qTeVlH+d7>TP22hMJEOE!M}WlfcjdcYNvTr+ z)kyxPJ#Hi6&2+*3wyK`X7&u!yvtN6Yk6$j$%4PSRY#pWLd=;^wuZ-2GeO9MzFk6Tf zHM|vJ*v9(WHcjHIaw95@D1qhCS^hfqz;N_d=981tkrGkf?ndms@o`jZq2`_G;fUTj zHKi?PvgO3qSqovsSbwUUlRr0iO`0e`{L7qlCx6-iT7i%F?P^NJt!r}q!?C-&(pS98 zdGwLxZtCN;ykaGI-xO{y4rfiCYiI(*DdjY!K?zOn;vCiIwbHzfbNL&__!#GicS|&{ zf_D4Mvt478tWNVAZ#Q8x_**TOd!6y`;tph8AG6+~g`R;BIN;Cb?Ha55I_V?D$pY&e z1kPZ+3w<+@K9JsEVmWYNzWX8cby i#{VN+xL?#74j2SlhXR3c{=N delta 14755 zcma*OWl$bX*RFlxPJrO9fdIkXU4jO8cXtUgxce0#xCeK4cXziyaChgc-1ocR?|J_0 z+I`eaSFJT?_f*eR&00O(T^SJNsSwDDGEmT%AP_tV6r>%GtNP^A}8a1uc}AObimPEZZ?^w zTPAR2L`HFjcgN447Y>PwzWhaI@)1=dJrQ|!>LxgZzNNkzhPBjzqOuKlMgnOV?Lw2# ztRu)y^|1d+#)kVc(u7~|AV#HxcG&Ip`1ITSHQb^rgmBFW$=xdeg0E3f1WEl8h9FIW zh4C}N#{?x<{=XT9n?Mp)1s3%>a!dDtq+aAZkTf)(7($2G@Nhs0u1fTNIt-2R-VSm) zNz$btsl!dF?)4uLaX%8&AcAn6`a9SXd!!P9)-F-GZ-|tNs*$^0igY~Q3kML~`z>r! zmkhRV!V)D%4utU8f!U9fX1D4RPW#i*f6k#vt;tTcv4OH}h*)AK5J= zY4FBTO!l6J!>DMxwV%jW|9P5`bUKNh=^2R%C&m`M40fxVQQJ_d7|ed1dQr&HKW?MI z?uoWv3u*(6lVOv}Ol3(%2c@*=+ZAg%4gqQNUO_TKvliqAw&#@VI&5?;oqDtY0;77V zPw~Fq3Z+)@IxO8#zH<3jrP%$$P;~z_n3Yq*Kr_{|drJ)ZcWbaOgir=G%<6Xb?Eczh zgB7S4?y;fwux27OzKkGk)+!sHu@wi%U{~s4t=?yfUO$-E2^{g;u}ur{`ctDqeoyk9 zSzC;WmiwT0NI|yZ>g16t=A~K#^-nWF!tO08__IwKtk%M)z=s4D2>O$!MXuwuU+m9J z?t1o%^YKCo&~kHheEPIAdIFy_sD{?BPM_v&^ENFWTpvc^y$DmcthCVf^uKun88li; z$_V|e1>}Mh;wfj-Q(H`_S>b#8RATnb!&z+7wXftRSe6)FJrJep$g)bGeuOb;7zi78 z8xAYA(T(U3PX?h1KFrt*XB?5Qa_~43b*U%9l=y zA8#BgT2wU6a1jg1n8;_;V?!PUKD~aAuPw@6@b3e9Jsl6*`c(olBS!G(Vi5T>2_puPM;X1sg~ONTc52}j(-K3d?i~zb#^jpF@wwDi z>Z7uJQkw3l9zIBC<4M?!@b0(cc5Oux@5FryglpBH3WT(XWb4DSWlY@x%#~;zmE$KX zpEf?@t&KkAG6%XNh=n%VA7qOptk}FT)k%`t82&y+lLCXxmRcbqZyB&}`~B}$UNf$ zIjORp)S{Z0->fb%#lW2!5GY0m1=s|-z?06;3)V=gC8fK-gM=Hz5TX6%1#9?~NI}rc z#L_q*wf7-4=@t;0NhzfkNaK}a_0u$@P@FAT2g2Ys_QY=&5hUy;Bm(nSM1VmHf?h_E z#{4Uqkje$2nV3_`{}r`TIzkG?=>|XwvB=dx`@NEw0E3h=Aov<%3`$VhTl_kh6oPc< zOgaKo^(1*Gq6FP3bQ*>yy+j1!Iq3OI>;MFN!qCb1{_Aov=|JNmz(*MKje1k1b6 zpt85*D&c#Q2P#GgZNJ^$E&NA__4^Sm=Ss!q&CUg6 zARyz_CjhgG6T;xO6GL=C{0ghMWW14R(V0;+!`dC?s63HkTZkx-Hy%Euq$b}tC^r|= zna0o504YufTJDE4_mQ9M5t|c=(mZb)3gWff3Ab1czBmSP_|%TNX@qPo353C|pu*DO zhfX}v3i}zv6cu_8N&SdByJMvW3W%FSIdi8xD!|#d!EXfRR+R<8h&x|gNusc(;~XVaLYBU-S7m~RCik#jx9gGR zP%8ca?Zg{zGw-v(%3kzuPveoQ6#(ZZVLTT%h6uy!hqV{3dA;apB~w;n8_jJdKY4h_ z!e{5;5<5>gR6k-_QtZHGA;~|Cz-ze3_pKprJhdNHDKqv%)bkULtB(fje(;FAZ}98P zhi0W#nYA0=$|))S)n6jv4qU6+nb3oz7&B^!pTGcb&UOa^EAI z&1sA)i;0+QyXwi}d>4dt{1&=;?O2)!=K)8wg(wMmAIvT&*L@+H{aioVzL#DJI?wwo z9kKy8n43kDgkWGvq7uYQG|?v*Gih4z;rzRxpxU3q&!O8yX=xrmd?&2|;!#2Q9B9(4 zDUpkWjW2XUc?=rZX=r7DiZ{53IE8J_{1Y1W z768%{$BK%W(e*UhvSv?ga!3?3U}Nl@?B)vWbcNjVOG?PcF~c5U7>@T!Rw=$z?n&KZ zLRM(ciCW{HIjh4@nYO8o3VYNsOB|!QxmT(7`RG7~h`66M{p2FC#5VPs* z$ANO2*#4+J{a9`5r@k3{OZmkk*S}Nnj4`wnCgoU&}G5y5QTdqu1 zS^?a6^zT|lDaJ=YfyE@O5)2FS;xI_cuZtx?ldyDhBk;` zf3-iRxXdSE9-I`|A#(AKv5mm5Zb+adczUDTM)aR7J;}Kg9B59Iq63kF{?^XH|E4WXbgIkaCis0Mw86J8=t zE@4BMi31WCl0nc*mRlyp73S!Dnir(}`nM@YN+Kj9G!q#vHNZ9hpJ+%r2~>p>)CMhV z)96|1*g7y1W`Y2chPS*`{6rH`wJ{RbhX5k*588T#4$-M*lq+CLa2B6-g#qw7ItM~*T?OmwlK~gna-A6!7_8#(~-OrDWrnB1US=U**i}7?;)u!wb zEc+49o=a00XaG4)IAoUusq~!Z0Y0j_5p`q(c?j9&3?e9awB>ZFoQ_L(!Sl`O!z67O zh6|4kk_qn~6KOc45Qcbk?Zl~8yKJmzqiOtU=p(jdo#HKW)l_})*P95GXN^3*>R_N> z7c)ii@53xz^5E8GFqarq1x8Ob&3?kVGaz1c>NWiQ`{ z=jW(1(yS+8hotbNH^0q+wLyQco3y7}q%MV2L~j9w2(S0;_#dv=d0j-RT)Yu?{bTL<4g7_Ymq#HWNX}6zI-}@a|zok&l&IU0N zQS28fne^?MGK*9>SnYhjBRq?g9a3YSimA&v{UA#zHbN&^TBJg#N{3>PH{fQUWX!#T zq>PzHuKty0@=xKuA+UjUbL@3_@`Di3{Ctbx$5uxgFdl7gP6k%K$)mSNA)ioRZlEa0 z>^vv8gw$N>`KdY5xu{-DX*ICD2;T}ih-(SH7cEdK+n2tlJ!{i z)hVbF>Z2tZEn@ksV6*SIZ(qd>*QYdloK&bryaC&gmf?(iA4DgmL@5UW1l07#yDq_{ z(XXJ`u?jdE8Xy9@LMV5z8~s&KuN>`526*BrAZE&$v|v+EY6+JVyDV&01gq>NA^*uH zJ;!!2g~Z;N2u@e~H`4+Xy&)WoMQv>yQX#21167g~42y1J)l&hJayz`&^+HDTEy#NO zUJ%QiTbnjxG7UpKX~z|6VO~E1=Hg2FhVn;qitv!^7SJKJ6Yz6C-UxyaSK1o;j-hAW z4^oPJ*ajOF15+qhTC3KPze2mDNy5{BdeNuY|U^P9DRP{Zlgi z!5ci`0fb2l(D;p2>(zF7oqLpEWe`k^$SQBMgngouj>)9_%y{vO26oJ`9>XbssxB7Hd=s# z@uL^js4FU}awv#z#1Oi`M)>)xi}8`Q{y~cHX?ZY|jx#cKQastF6DcjJ`gV>%7}_JpCr}_GJ%w$KCul z;`*6Jip#P-fwOC%tS|bBqNLE6AcT}2*BGw`BG=s%<4^HWyo|zraw%%ol0k1PfO9at zFB^Lamr^AH%7?awa<>>Uzz4mYKg!rDKHRI{o}2TRe@2$Grzm+lH}-J;)XnMPs^aH$ zE13zft3hBUs0oBme-Ep}O?auKZMM>f%>E>4-^0DMGsP+t@~$pjXE538rm$t-MtoxxBhH>X z4(Dsm`*Ni?%>KexkV(NqL6Z%sBtFLM2k*|49u5#@jeaHBxb^id8$q1%8o_!r&eFdHaHy%IJ%~IG zw9Bz8wRh{ru8k(K3>{H#5_D5`)o3rbHq~uD=f61FczNO$@1v081`a9Sa6lm^AWKj0 zb8R$G$C5M?CD0lnWUHWOlY79~zYtC9lymziB8b*?PAQ+F@REJ$8eu>?^{-lz zOSw0BYIJOjU*eupCh4#M5@ljn>pzO|tpQAT==bJxn(___IG^B^77nu=!@*RSt!o5hs7 zs;_>xZ52W3?Fd&dQ6R8@Lb7Z9qm9hgIJj`tUw^I?$aWo#r4_S!`{c2~6GD*JPbc3_GYL*tD8BbrsNZ|_+3 z#v!71C0zaD3g;}@tc3oZhaOUPm-A_B0R|4C#y^uP`B&b|!w$o<_go{O44p*GKTKGI zj;yhL^ix2$G4D+m-OWa?hT1OP?vN;18dU`u{Ruq6ioweE)Yzd5bPl&M;RwvzNFjKP zi6*^xjf%q+6)crscW)X(yvpqHG(92KfAj4B2`l;l}S`?oM8jrk_S}E`)n2yeYgtcEDp&-9CCx}#)3n~cg__xVuk+u2F%tV$8|^0r{3cxYsZ{yx znTh|CvUo8U#&iUKzS;vpDjagl!+I2(nJ&R?+i8#ro4Is*aGUE~$tH3MMH2Xv(?e*1 zzKZGyvvsStlso8uTY2CbB}IV~e% za+`-E%V@JqzDEm2>)tHNMVlqZW@wwppLAYL{wgo$$Yq#F%(DU-C-$W%Jg6rKS3 zTqG?-6P>oR81Py30C~8R`8*dmdvAM>Y^Kv>?Cf0iiju^vZVG;u{3R=La4GrVq;I`P zl**K9elwh#L@GKFzr8j0v5yn8d#xikezxDs{cQSI2y8uc)e0KNPh9}*W%cJXmqToa zn*3Gj_wM~o96wDsi$iKmE*>f^!{Z)A2Vk;&f>8^HBh=m)l7@GsXmm_(jMikv)48Q{ zzc0b``>z6nWfc?dM9N!_js$CuGzbX>T{|+C+-V?J`^`9>$ z)Z~EKhRt?>8dKTT#n*b>I!b4z?<=LMu-5Id?q^8x(q0LT?KrO`hl}laaES zAH9PQRpb5Ttc;c-v;w5^7>?(+`_u~Leg{hj{L;_k09Ym;{^&-y@V&J2B8~XkTCvTK zZ=hJgo|%k#Y@SP$>PWx{DrUaz1;19#N{#kDU<--l#2ce6hl8{A*lo* zQQQQ}DYTIP%k=ppV0c=(V^0Is2?BvY(&F-B6(R-!|MNQn9v&VPqz&f7B>|3|R-3Ti zHVd~bR^@A=Ndw$ltj%jap39n@$Wg1Axj$2jDrb)|h}Te>YCmJVF5e%6F}LK}!pKd( zA{L~NaV3O7U2TCL1h%$7FWaZrJ=?_6Xe*gs+&y6=j(zOyDW?;Rl39g<(e=}-rGl?p znn|_^ktya3IDOFrzxE|=z(8K}`USakJK-0o`yN&i-ZqD57z9T0>z?cIv_CC0U-kPY zzg7fIj~u4+V>9f<3^~uSvx2|;VLxjXN2!D3Z^1V@=qUe{D+=+^p;S)tg6e}cEMV$d@%WgL#; zU~R%^+esPSd_66Ki*8RBcUn~R)e5Ygiu>;FRcoi-oqJsS(`H!3%-}gAGDQmK+AA}; zbcBU6gI`U3%hNL9g#nNfw8a7tPr{&K9Gy#>EV<0=9PO)zEmTkyiu!)?)qKiwCpQbL zA`4`aPH^6KSnxR3x3@nVgR5a$=y7YkSaRzfa#&14z-gFm#xAlE&12VTbyha0sjv80 zH85r2=!t>0iIm$GP>x;7 zvh?bmqeEd0d1|1qR*Xk#Fzb2=8s5#A`y?L%-)v#Ua&-dkGFxCFTsRnvjN?FCLFaqV zjpHpYWHa~g4W3q&SdxZAp?-uh#`7#o8h4Cp+MfS_a5eDmJna6f?~lda&bRN~EEY)G z(=rI5(LToV5@2Aj>^eKil^FLe(AF&3hr*yY>|u~=xNqfs4Bjm7q{A|)S|L&{VCWN> z<5gWheSz-7j4H7kS7hp@`Arwt${1Y#0mC z^`AD&sJ5;Z`w2IY1kzI?xI1u+Rqq!Xs?jKenX~v0i!4BtjkPM4w@EP5q7hPB!|sV& zncl#4mlwVun%A2UZ-yxsnOr@eRT&;ZY&6BBxt5`{nH3hsSv!r9MdRmjGBwO(@p4M& zVdEK0tu(NRt6izK;eE^P*}M4I!Eg+5n`xiyW6jr%`cI6K=a5!29qel=j=iP$mrUyn z9Fj^@n9xAfg`r9to0&R;Bxi$62mDJiuN%&tIEgSoB$5|5@i9_#vf4bD?y;8sX_9Rg zbCOe*H#c~I?>+h22GLUAo5@Ti*vZq~g6gU}hoV0`ODWw$0f@(Yz7wD7;KT365q^h^ zV}>76jOn82e?vmq?ZXfI9t4|!+fuG%ov5?}ISN=+GMMEIXoUrswnxmsCggtuGuUuW z3LOje6&EkxmNB228J`mD*y?hQw^jHx@-|?+5?E8&*xaASoYB)!PCszV|LG6eRaL%{ zS;gIhIBV?ou`e2gyGqWe_D5%o;KhH0i75q#e3d8!B}#^j7&mRnH&IiF&vD+Gl}Yp7 z?EyGV^=?k0=O;4Qbgxcu>Q4Jk?|BlKbw4G_eDsoDzE-o%ZODe7G=DN2mFDGZ49W1x zO4|*e9r=xI>$`LE8DRHje0i44YFf+K4jG=*Nvz-Xkk`Rj`gAI*;D(z->CMYx3B!-? zpFzK5`T^lo&|b}!Ju~Mlp4?Dk_t31)ka$W*IBwsYr@Zsp*dY?6u&z5b@u*_q? zIWLcSqi*4k<|SPJ8&qnHf;Ex*&;n1@7prx9hX$^dF(a*OCgch1eeS`Gg*J>p);1>G z-t+qY#dM@X^M}*6YIW;*z7L|c3ztAT>TK$)p+y7N?}O4kd)&+~F~=M~7uI0|S%*C* z-U@G*-w!q4Jirf@J{W=~&ZmR8mv)|msI#g!3Bq<4MKoJ+lE3pKPw!<0$nw8L|9iYDzZXZ!#)YZr?N z(X6RI@v)tHmhLzm^oQ z4AQVyZJb|zZCzXqCsMe%C5bC=$E}>caA!t#DC?-TnHN0qv2c0UUg1-M0#Xsu@S%p+ zjE9h9s>AuX+GFeuWlSl_?+ZZ5jaU-HA~kA!**@LAT<6baaUSkbgy34mTPh^#m=4mi(tr&sdFy6)5=5Yk1fNU){d}a zQCA9Q`(@K{0OJl+P=xLDNbhvJLA49=f~W?Z<%C};-rJb-W@y9nQMq_$+tn$;!oy-^ zN2ov)!?Ds+r*!zH2%{j z_aIJrT|>m`;B>i84#sA@fU{^h6@76<-YCULVx`4g9`W1oef4>;o0AP8Rz7~r*W&Gu z&UtZ~miHp5*i#+i1Cz!hq<2QcZh36^1e4O=XTW$H_5*SBIq-$WzsF+h5j2A(O!~)% zo}!8wxJ_nA@J9eZ0LkW8BdiLU%s7Z$J*$`v`b8bRQU(SR4Q=n4wd=GsOR%#Q0v7ZS zxFD!H!?LBYY9k;@HI9J}CQ3CvVBQy(E~;Wr;B3NJO-Bsk`q z=?fhO^AALzzO(g2GO}hSuIP2VHbC#joW^?VbZ?T&FCrm3S@nmDV`Pj*R5DFWdA^2H zLd?~C1letF0%U?0Pe{ImZh;z4K{n%OR}oca21VLmIAYCg&flIVbo!>}Z(Cb8n_Bhn z#!HZR;oGtuD!%&C$TM^EV&|uGQA+Hoj>+#$tEMDZK?WfQAqVm4h`gabLM%pe zsAfR|#n(DW97=F5u^4G|#aYFs9MIO00*M9s2V_6Q6;d_3pe25K78O!8x56P4ZQF{XsmY_#E{u%wTCIL3t>sL!ox40sdX+UJY&zQ#g8?Aru$dm;?S$<;tX{YAo%l~0HD2gs@AmWa-%hH{yXlZGfF zBa;wT9zQ}qfY6T+mMLuWy@ZyQ2!bIw*(dlawPejpu0BQg%IerLEG#5~aTOgMJ17$_ z_bQKWvi&~F0G1N;4-PSu*+~@ECi(Cp@vwES^xrYGoNo>uQbS(UP{b*dvw=yKdhz8m z6)L*^Cb@!hGBTCoQcvXDE|q_QGAJPia9d;DDSvp&;pTPJLKwQZPLhte?SpFm1q!q* z`W)o2eGglxnCeZdM@A&=b&05ntqA&uE^Z~E{>>h@6g$@@@92BG1y{_*qaupazI2hM z{X?NK>fPop<$rYiFKF#@hWV!ss3P*N4GOwYhDIC{6}Ab&w;emj4H3fR>#X| zqE~R$qTJnN(1f6Wm|9-WB{OUlm12ac^TrB|TfrC%O8gphfZ!420xdw6O`A=dUE-Wm zk_=_@+^|czTiEis*{{-Rd+v`3Le9P0Zma%AkA%rm7;@KN+NcfA4vXMa09ciO**JI5 z6n|CFgff8rhqn-aPm<~{8WO*s+GQ<0B9~^JE1uu}VAqWRRb$@{Zt%$a1LL2AQ zxD-Xs<78u+1_g{cQ0PMIDDfb;#e4B6lP78Z>J*&OKJt9u~7~=#iK!YJdn@P z?-nGY#Q-^_5kJ&)v4Z|IxSRT}PvLykYtwS#Jd`@&&GSFN*<#pY*sADuSX6i2S5^a; z$Ws(B_5~=Act80~_)Sp9O!U`2P-mJhe`+>JwsG(1_#0aVn@s93z>9wXlViTNJqaqx zz`>LBmZFNHfk2Nq5TJjTC;mGFktznRpl5;{!vk;869RFDLsedG$sK+gNiEvz<%QZc zy(9n-br2eyWoC#0G?%=7(($=4=Dh6jc>05wHZnM=4!l1B0Of}dImesd!?neH(}A%U zzEFl`I9y`$#`#l5b#eU7Pj1*|jnzbt%eH45pPU6ZkXzT!d`5#+zr?adnd?^s@MUf4 z=_i{$0@|AIU!)5c>&OCE-pwCG=nV#uPVb0@QlA~Y z7&5~F@nF?>N&$WruGWry%=#lkfOJ;m^&-5083hos0H;?#?8^dc-Hj zz^ny)*5-J}PUx&b<&*2>?0HtqpLgb(eUjd#YZ4!O^(6hXdu$RYh*XBy=_SV2jhIwT zAX9`!$H&zF8}B%V75?{i8{1f-JNX2~&6vuLJ(t#1X0!+OHI8mhyeM#D(8M!TcIc7FWp$?i8U>-TG``cXtWlkf-TRh}+{S8J zB8))1WjJ#w%4i|T>6Jdb8E_&V6`GH6BA0dw<8R3Q5Kics;dWd{_CXBW^NM<81Web) zJ!oz|o+JM{FjL~v-Pz@CY6L^-Eam3#vc3-L-1P*0e&aW4@>4SyRNj;-u^q)D_mvWz zoP_nYqWt5FvzZ=tW`Mo7X^5IxHM`!m8|G_tW20d${Wj9u8Ne8sk-wykpQT~*_Q6JK zqw-~yAYoIXvR6Ai!|YA?)VinG73gQZ?rrIeZG}v{)Q;$ky-w50nn+FT)j6zI6Z5s& zm;Nq+XDcZ0Ve2k5!WhT=h%;vFl@+sg9r)aTPl>+e8&b3WHQ_*DL)n`! z?>ckf)HqbMagiGDnb*YE_a#SlUNN7~`F-pVD|PY^!`yeJ!TEVn9b4&VFCZp|jkMIn z>ri!-+=-ptwYdOUQR|BF{zWEzHeZAcyrL-ctDMq^y81+C3Oc^Cu57ped6?B%;p0j} z?UqT(MmWnWTx&LW7}4JIad_$y*GgDu?8OgCSCPr<9qRk`ccF7s+7+Y85F=Z<^S&xO zDi|=}XI<)Sv&$nTOHlq%GxM3T0 zKUw4V(S<2A8@2bv&qMYWpfTcPoTxuNGJEls_?m!sJLxRa_C=^>eVO~NXq9Dy@7T8y zZ76u3dw+K=kx8(6j7v!-=Cm_&iV5a_r#;o3J&^^@#&(wdr;k2LZPSr|e}Aq1@4 z|Eao z-%j`ODn%G0=KWm@{-%PQN`76V$>@uNu}?|mr%_)5Io){rQ*_N7K?D>mwtlOsi#cF# z1n;v`FhP}g4#k8XXC)D+#8(YWjSfd zBTViBO43_aCz;QZB~fdibsEuTWGHjpa3JmzbbEawcyvvUPg>u#zUfLT>E%kuSnF?Q zqXx*hWO+M}7!2H4CC@<1wN9NMw#EV~QG9*7#&KR8AHPllZp)I9J~YaBIS>FxoqHVF ztQU*Pb)jadK!!)g}uWLVnL*DV$Me7;8vDWu|k{kQ>v+woddxbx1DSgw18lIVzugF@@h^K4+!+|_#aGdey*(sTw4c9T@KyG=v2VHCLiOs zG|8(U>y0j!;d6M;dp~&y*}vM`)SXPDkQ*06Blp_nv&GFQR~g*MQKD1l%2<~T|I&ec zlWrkE9}fR9Mq=&UMdr%#T?axCWih`XI1#dfph8MrssXYv?;9l?PmI*zoEK!p>>3_= zcl3e#$BS7E%j*8TNUYBesx^IpQSF`tey~76=}ZqT)yY86HH&0jm1KQ03A!!IS^r&8 zwMazujl|DGYHr%-47}a=p)}s=UjaJfLX$g=g>a{^X`%4V1s&yunvF#=^$E*eWikQ$ zR@hQ%R(|)7CN^W#R0lMi;#J^7x9|Ryd6GlQvUW(jW7+K=8njfof3vaxUGfz}w^m-p zat?hfzw`&5Y!CTCf=v3zYeOKS?{kqu3DHy;dMGy6qiYDL#FR^Q61KWS9vU z{xM!FO!_`@Hxz*!2GM8-9Dw5`U70nOD5@4~&uR#ikp*%>ZO!O?AbEt}1&kKAdQMCp$2%UKviNc#aPLO#S$6xC=Fn zD}Z~ZNBIZ7&Sd+VVjGs->JDx89iYC_0&6_Xh5CkMMS6J|xF&5d+u zk}RjFueaaZt3YieWSge%bFIVH;~Ac+p$2wApli|mZz45+fE`Vjuwz~&5rQZrS{WrzKJ4%0?FO`BG;yX6aCRU=fx=25Af1j^n=yc6V05+NI5lIdel z+F=N|?^Sv*Kb1{G@e&8=Zn&`NtGLOUR9iW%+aZ<+36SY_0j;gREy?Ag<|B$*Lpcp{ zJP{&155?ZN>VXCHGEwm=y_?1&cBA7cr3>4thUOAuIkKQi(~k?-^Gg$V>u-VnYK%~=vuMw{So&65n?87YL+ z26B-D$faMA0gs%OosALEd7ZW7>uz}z7s5&6#!Fe8v|#N}c8)XK3&|`Y`ewd?di=7( z;5@-WMr>)bXts_Jlnc&YWy+6TE?uSQgJ|Hzb_I?_16z+2@upLVycU8s$jtU&0(RJm zvMab=DP<7xH&)7@8`{Qss2V>zy5hrFY|5DsZ0uqofY?V5PXbXnFFrUzD-nioh)4?% zzw))r$v@L;>?|BOGUMS@VOw!8xxuo8KpUH&$qlkbFqe7a>>XD>_$CPkJ;&`6pHusp zMr8?meS(nH*{@1r{`JGh%bI|!8BcR9X@Jlv1tf6dFHcY1Z!?C*gl5yrY(5+ENIa+r z5owKf4M3T?HNd9Z~^V=*k>^Ebrbxo*4q==K66`+@I0tr_ChuCzC+(7 zrO9aQ$l!jswDA|jkHRq^H;Br;m0-Ox7!sRD01m;lYNkWVtVq^KA5N-F&lxU0*u<;h z)=XRz=%Zro6fm0A)m{dN2d;aK>=8$54ymUMx_BqSEEy?==xT(p(V}J0h_N*Lj*JC* z3Nxe&%bKErKlPdYsVu+rd$#5u#$Y0A%$X3j)qy+lp6b}C8L?iOGUy=eUWvNdug?Z` z0Ftn!1<+kLpoQIp<(=ZJ8H$0(C%7vfX405ae3;TW3PWn+3hP2zR0~vV2Rn!uO~gan zL&Qhnk6o3MZdeQ4NmgYcOBIa|9Rw$?fxDSOFe`g)#cT!~bq*8;&5LJJ(_%Jr^@%z{ zCtnsG+>BlFA2~LncD1r%DNOn=AP0Q*f#;nVy$354r+a+PI2U`wXshuO#8TeJ)C~jE zU~Ob)VW3D`F2%)VtU6&4KFwES4${0(z@(`%Hhc@7Ac{Pg-#5ON6gxk)vAeAWQ#M-{ zC)H<&@zd@Q*`vDpt2xD?MDQfpZhp?3_|>t`@7TKxZ@?<2(8sF&!=4v<1Mq$bI0xQx z_qnuhZssPO=gnuOZN}g5o$uT)GtGYcjAaABcp%^@@(JQpq2By#xEDP45EtyZHonj^j14vr zsCn7xz-gHZW^l<7WIp}P)k$0he0bw)&&@dI*tD`oS!tPco<1;OuspWuWjygj^ZloX zzlyiA{Hwl+`#fCLa|rI^d!+K)dd~6d z-17j|XLz}#$C4-3{nE1c{UaaWJ3WSys~N{RAJ5Oqx`U}v#ni^#;IJ}jjZE(QEGKr2 zlyK(5ttTrhBi2rUH+z(CM7*F8{)YqC+hBXffp1oO>(cSjPvptnaf$`7w(cgKqWu~U`>#{%|5(W7Xi@*(*?(*b)RN=U{A>Ep6dmdB zl7as%3HTt8vAvy(shtahhpmmG3x`_Z|jshz3L|L zAkY#s2!!}w7g6$nK+Z0nHm1)1+4#TvCI215JZKQ;?*ac`x-JnA$kx=w@IO`mXHp^D fJ9-@p0tDKC0)enT{Ff|T5)787hTWF>YyJNK5bp@o diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 0dfa43cbf3320e8ac2e3d8247cb67dac7b5885c1..3254d6140b3b516870c230c610f90940800249aa 100644 GIT binary patch delta 31923 zcmZVhRZyK>6E12)aDoJaGjR*>&;0jTK&3;N{Lt#hhIi4>ng%Tf(eQ{^BA-nciAL>|I4M6x3A3P{xqxb zBLZsMDDV7^H{Uk{c0i=`_=ZzCc2$M03KkTo0wuzr`==u3=)hr%HF5wGO2Cb;&})Oh*h z`Itna0kT=#W%zcdCfC*leDA`KH#z4Z67N+lM4dpGPZfv9wqByPcVQSa45U^*eI zirqdhLtZI>Lj&N}I;^cdaCrmjL}&L@wA^@$vXcnoU};1l8URe(AO$x>qQoY&D#txn}BsQnH|b?H*7-MZVXOu zQ(GtvHw2nH79*YaJ2h4q9pDW&vRgXX@^T)^4PBg@YP&iXcZ)aAqnI5=+std7iPuxO zZRU0_ zaO8!6$Wr~!2eW}j9kE-p5bxDs-(`7Cjc+Xq!|}e_;JYy(5?gm_M&k-XIIsAIeA&A9OO?t zb?`qqt)ZadyWC|1m@7Im4i$cowo`6=HYbDXz}>;Xwzz~N@7Ht5INVkDxyp_H5FCj` zGs_+l{Bh94eIO<`2WUH`RyhT7yVTDg>JOCTHZv5K7qHXY==ota%ZN1xhQ#cbS!5t5 zRRhXyWH_|u%e;%~{|=vPYgNdqLDV7`*AORkKtSL{P#P-hQ~@SfI&X(G`_ z73mb8O3C2@sYtnhB@o((T!5kMTb*F{N_<;r5Qh~k_eu!jUst>xT5WV-1W2v9oEJrW z*i;sq;XS`cxBG!@Ji{KU@yZEnwsXGBO9SiPA}3tYbL%9k2o!Fm0dt+_4-};L(vm02 zpH$E}RN@HYy&DgG)I7C$O9e|HDD8qzDkgo0br|DmgsO&7x8L{nz%k$b9AIBg5!vxu zU2YdKMp=8mC!ejNGYm)@47ZSIRU~w{Zu*l_Qt&1R?BVjRB^ov_q=7OMK$0TP@_?js zaKK);KBHpQV5J>0(szl6o<&#u^O-?77J)`-oid zMXxz~ACM8?wGzVZ+0PHJONw0NMz3|k?p>7lp)MtCM6db5o(&I6UO;!f{R@=5*aow_ zq-LtzoYNH7uNE?MQmv>Yoi*$KTLM&)&dLPEnRKm3FV6DyV&{iiN2p3>J%`|7jUbIp4*l}AqXeov_pHpw!t)L^0N!r5#*cgckgjo*4i0uhk zi4MZykqOzKKtbWrdstg|;{DPFFrXkhyb+rKGo@OAie`X zRg|%Zt<0g-C%{aJn<2>i|M2hafGpG)pM!<;kwqZrVemfqFQ@L~;Qi3K4VysWMa(IZ zv8UuZ;o{f6tLRL9a0&5jyI+9C+?x9<)TB||VQL$!rzB&j#T@1qeiH`;=U;)s?_4Tk z0Tycc>q0$jbgmSf@PL-&z|SH{6zQ0Qy`rC6geXI0KJDHj@!NQA4w{OH{j4V&HN;=$ z9i!V?DrthZ@tWXyB&$PB<_Na%n($+lbpuT1%y}g3pktM@w(*(}cqGr4r8i(eh3;&L zk5EHporEJ&0VZ?G(R&muu{(iq>)K~E8}pKWF1Z|;{+i^fhTK~keQq)#;8P*El=zii?E z2i_JwD(a-=(J2z@q@`u^ZD0889Xf)M*+?P_O=X|*I7t`C8{O#f*AKc< z1Jljo*i}gYH#x9=d}m;O&Qi``3LT<77sPdI&MLG~UU`S6 zPnAK=XmEPf(HxkRJLHsWdxO%Xk#V9XPEoM!z*D+8gMGrT1TMvDV@zOAB)z38bruWB zbTvHX+3onj48YrjQRZ zU6+nbPxVADWp6tve<@{eJI|xJgr0b5=XyM;(Q(*I13X61+2>Xc)0Q&h!6REP<-RmC zpm^k&VR2=x(mCEGLwAXsxa9T#m#Rv~pLJ%+ZoS(2$UV`?%xeE!f3^YM2h7mV3iEGu zZQMrMGwF)~BgxD6Txj~Gj^C(U!cK|CEc9iN@Ia@>DpOhn7T3qIbfOpkJx7m8d8{G9 z8Fp0;zW6Bvk{}HP%#Bl%`uV`%r?&JppbCw0Y;ow1*;+biIxx6#J$;UXPukoRiGo0y z&>+u>rEPVv zlg#J#iN2b$GH8vWnVP#Ys3Y&QMO^B%=mdstQs}&>H3m_UwHs;G&=6K7jIkzh=pYLp zsj-t#Xf&GiQbF8^Bm!mC+`yeZpaE)k_9Oc9*g_~Dag=Xi;#77Yj;6!T6-FT8Iu*@P ze|n2CVx9jG4!^guK4>Hkq9P8%Q3b#EJv*)r5pf9=JIF$ikM)NabZg8Jz-i>h zTmR1z3Z~wu`KBLHOaBu)Q+_xkK*OIG?w)mI)X-b}eWnUR#bSZ1^(+?)3>(f!4j(s0 z4X5tMFO%f4{0|bAk$RA9U{eOe5Ltt;kTJthFUB?x$W03V>oyN)nHz<=&xyz1(I!9) z%}=*qIda^H9E8BHLv#n47FEfv`&ryl2(mLnAhK z3JEQ=P0Zz+{_sDbu80}b&`&p~erYj4gm2V7kCFQjf_w`IVeH~5xbA-hSf%a;EEDCi zr0!nLe*=oI?dS@FaJG4r7>5E0wt0M+$ByzPdC)cm+4=GjhqejrB@wW;d3@t0jsnO* z2;X#xjDmNsVnhmIq8&k99+s{F7^(d&gfVEOea=s4#oA6^jCr9__o2iAEwoMeCR+V2 z3tp~b6s&2so7TgyA~r_UAC&Af%!XZ9@49T)Snn-{5=mgFNxwvPPGTU#tgEOEoG2*$ z6HgJ>&P&3PzBQcTT7%LBL>Uq)nPs?N`OoKARq?K>x{)T)IfL6xMkCl@i;DM+-|&53 z`A9-v<||b${j(!QhOv-YKJRLwUT z!;V|h3_x!}MJO)PZA_DvBLuxxJh4H6iM?y$AMc_^zDuh<)5up~BZ8oaIeo+(Iqh)X z=ety&tCi@*wqHBg+0IbbG3`daLu`x2lho3KARx-8y?XnHx5$6F70k1WD?9*`lCu*S znYR--`r}$;K=E3{tJIT}_wa7zk`kTLBHQ3KXD=`@Z!d7>$BoE{;*E$EjaU92jaPnS zl0bg8Rqkq`9?+|GK9YjFpEzWFDZpN&(6&FYq3@;4R8jSCG#cfMZQeB5)!{7W+w3r& z)-umeggnYfQtQ|HwIF|u!g*y;SEsdfuc?Ip`SYlD@uHA})qeJj6|)z-hEF%Az4pHE zj14deI;$}g8R`GClEh5km#RKN+4*1vM!vAzJK41M17cODxgM*6T_~d)X-&X76VqHP zp$~Mlhn~cCstkr^qb-mdCHHtcr{tKEervJwNu~9b*#~xc>VTld;ao_!P00UbjAjBb-3k z(5Snz^R8k>NOBAtk@Fz=RP@l#ED;20^y zWt_-PQbv_}cZ_l~ff|M=j%{K(ioN$VU9p+r5V!EN0Xh4g2D)X(Kb}oT=#f)ET~c4h z@|nXdp2e3}JJpY)jK-12_HQ`1$ylq>o~e0$HuhDSsR0)6Wttk!;a5aSdH*7dy`AgK zQch>!%agBPvdfNEglCt|`0Q`rlv@GSV-0G4O)1oH;0AZ0Y4RWp!;r9E z*T<@eUebWTb_>CILTLGRpM^cZXwQUlxi3apF)>QC66Rj3HQnAV-=7tof)@gy zez5FuWk5?j#!`e~b1>Q`+hTKS0jgqhb0B{jirNq-Ik}Vj`!244Jl8HT^9>1CRMLOW z5(SsN3NkyxkEh`7Zvx>Ba@KUD@D}%%Cj70;#`LKQLJ+h2GqaAfj8&LevhWNS{w*Vd z01QxAi~D!7|6w7Z**E@1Xanwg?^&a)C&B}Ho=EiL;@5*%oV8X z)MBp|Be$P-JQ@NpUV%9pZ(0y1%g24C;{JfTf~@u~za(_8HW2@?#fk6MO;wdLdPfI4 zRs3%{*j4*K^MI8(05L(?d>sF8BFNsZ!j});ZM5Y@qv5?>Q~;;$HiEW!7Q%Q7?9i9dAu!4$f zCO`O)LY!S5I5gT3@4{bJx|_Wvuj!KWmVB(gjbg#lNuSOBYiJ>d0b@<)VmaykXNNzK z^$)&(EJI^}A|-na+wLWSBTw?{rq;AC!H(1pD;IB$vEn>yf7lhwgMGQ{%L%BBy;`(F zh5R~$NJ_ooX;r+d!hp;F#%hrB9?BXX)4z3s*GDGTzb(Z9wccD;?3(N$15TpT-k_N? zy%t-CvyHvNu{E7-vP7L&66*08g-Rf&96*XcIu!e#YlJR zGMRVTThD(h2zr*SR6oTaGOFkMa+w-ehrcz~zHed+&vS3zidX-C6B^IG{WIPzBWEHE zTE#dt!haq_ZD=t5CwR}i8g4JIwoqtH%l6^_&tT~XZrH3hRIG1GmE~BCUgY7z^*IRSc@W8O*Gl-}|N1YxJk|M3u=pQxwL_u$~knz&w zE6;f?_aS94e^G^r>M!pkt>!;+{+oBy(ZzRTbkkZm^~^VC^XBwX7N_@iM8N@J*nsmE>=jVc_28=;vKEPOOR^ zb!=Uuv?$tW=6@VmY86X6%hWURuX=n)2kcK`pq9G$P3a;6J{zH=nTDg?6v~1 ziKGWQQoaU{VzoCN?4zZ8ESHJ7-igZ`mwC?BWBirA72>x{^m$y>uQg#^M(YOHU?{f@ zTfrLPYYkiZ*6v&19SyFMJKA0hijB0G-{o2CgVOAmz24!bocS$!g3+E;T=17pxs&=a z@XjKWR$9-h?L%Gy5?qMk!+DV@h6O;OpnS?AHiLYMDnE&s8l-osM`vh8E*_z{Sr*CvKRT_$KChQ68HaBDoh1**WiKZIa+1RfZ!B!axn*UrEQB z4$#^o6k`k^t%UYN`c`YACXrd)V z#9_v7K(-jULGw4DiKgvZo~qPCBh+CP#!1o0D-b0+(|>p4jcV{ADHXl2q$k?Xh8q*3 z2~w?R);^anz^tT5@II*;y|B0z+AlL+M2ys+fqD=_V1_mWxEIrySBn8G%De2wx?B%R z{%T5MqK%gMc8z4BjaCu>C!M;R|9`rYQDpYLQ9TGjGI+P=&;Lu>PTM}u6a@(3 z|KeT8cZ3lOQ?2{oRvIX&0U$8;Ip>>I1`!zZDz(oCdOrI`3*JHtPI|B&=#B|J2atkY zdj8u(S#WU2gr0-z1Q7mv6$QIOQOF}qL2~ho`Jhsdp~TbC0?hcJHraC?c`5c?uA6}| z{E=m{>tbJptHjmCyHleA={*h{=VeG%s#6%vx*;RU72CoQ&!VN(U0q?SFzXqnaU0}>5qGB#*xUP5D>9^c4`>k zpT?5S2lqu&5b!H^H_4z0^6~>M_4T#9uvT*0wf}r~?wHvyfCj}s2T4lwJ=wRAH`Af< z+Mi-~OowVg$p+M}zQ@wnY*+Cd3K@~5bO#!e3(w(0+z2O16%dYZ=`dwbY^+MwO;?)qxl{iQ#s;| z=RFVPeMepFU986Km}*-Y8BHgCtT6;Zuo}3;)HD?ae_ImeJ<#nhdN@NY?d7zZgW-WE zT2ssx;1u`y^-4T-k2l}`XFTq^RSZcdC6a+r3OIXMN6tDyqgs$!#{g6CHRYy~vd*xH z8_DLRp`=$X+JFibDz6FOh{c92H6oRD!oL{u@*|Z*afdG`SJ<+q{y^i?P91%Bs_?N? zgXg__`asIzcUPRkqIn4!wgFFs@C~3pZ2_P#Lc>28g!N?~q`O@9H z2-N#NVfbhC`M@W0!n8rw{#lhqEY768W6yxo##DD&m<_L9Alwk%JzQFq?Sas>rD~PD zpT|=ArtpwbPIT8FdxCJGkbc3qn%0zk=3`_0NDsjyjurN@8`!q@{UNZSp)Sy;*aF!; zEPT7oIm}+8A0uF`bopVEH_^JT#PojPJ;}4KP=(*p_GCL~mCAiB0I?kO!%Cct5QQiJ z3Gv#W(i{z5r*)uT42GYl*FKp?B-y7~cOCs9)_ZdtE-fuv(`58U zFB#S)yqSVNwP-zAhjBvf`~akl9WMEvpC_lgd!f1v&7G^6(Jyvt?-+<&mfsR_S6Iw- z(hh6&KIc@ICuOzXo*%uG>3r1{26z6TGHtMDxH#8m2A2D8Y6X8Oj^&6)-(bF9gei3* zQ8?zsjP3m8YzTYr_QJ#&X|HaGY>1cT3WJO!#oIyZWqgs6vw8&%;{gts@(G&WUUFa-eZ)TxwFu4r^Nfa!X zNZE>qu}ekwR`Bwc#h%4$HR?tP%RnXuerlmG`i; zI8&oKl(*@M$yY^V&4CoTDN0%`e!)4hb;@uN&|0mcLdWS1HIi4afN-J_whXzyg+yO* zanl~hfe?yEi$dtIKe?>Ue(sYbN|-KF|AEL z(&OfSk7gIgWAl(RBX+vj*bT=*LT!EcU8A+LZU4ChbOXu!81MqGybLjz-UTJlJz2%7 zW7o1BO&a@`EqXNNeL$W+@*p2Qitlep)E3#7zRy?L+i6_$cWy$WH=m0hRMV8JU%pl!f%aN2z1DV}QUT+uAX!LF|BNL8WV+t_rO7z;K!f4zV7`|x`_$n`oH%gA7# z^vXRHSk0)?1G=oaW~RLVk=Y#V;LGX4Kjs8{@%~D7me=ZSCi_}+^`+H13YrFquxwVDIXBc*93zOf?2JcJOUdH2`Z9&m}|T*K#=vhFtxpu*cfK zH%3JKh+;X7ssV;pQ#9FB;@+}t z(IWj_gas>B+~iukKL5`*b-hF;MsTvGnub~B>s|HnVmHp)$0@~`DC^Te2#r$oBq?4P z`3MglpB{TR-3PRt;}=k)kx@;znPCn5PKnZIu$of zO<7>2#((y(-4{OHFubd*r8k~mU9$C8zKH_A%3C<`Z@qLaF zlbd3Efi}r;9CV4f66byEy?V;9dC+Z)NsQ-F@>B z^U{OwQQ$K*^n_+cQ@_P@`g>n-+S!$Pada252Jx5*`ss>)7bsOY?cb4Rx_#0xeaI8J z=+wVO7q_>1`DDO@tA*ZiR;Th4L*XYLXx@# z;tt{p@G2w^`)*zHH@7?WYJ9|MGRO)3s6`NL4`bfB+}XH(DZOSM#uWW(4JY?k;DZaf z;N6n#?g5GCmK}objO|-{V6ZE75Jt|tweKEjcSnCm0~+G=WXSzaH-hOb+yWYL9D2XX z2|0l2x(uxZf*U+>r-z|0Fy8UsgSFxT{^ZBLaMLNpDaC2#KWK`Xee_qHPx=rGwrD+h z|L;QE{$s)eAEKGiihdY6(e|Q~`GAoyaq!UY=WBruG>5*HU7ss|-!CL&76~$@WY6^6 zak_(@W9T85;{I?z^&jQGG6IQNnX759eitOqD-cNz;v-|2yZ0Zi!UD_ou27u-`exiT zjaCowS%?EN?(WDF@{>V`LofO+&9&-Zm<#7Gz_0F}&8_a;uduzElt?P_vRhfy;1RRE zp*g9HJ0cENCjvGC_3ms z*qp}%_#+OvzD4a7uZ5t|_4rjFb2ayL_yZ}#2xT`QgnSORGY@(bj%7nPa3@#F!5GyM zcS!w7-dD)z;AaOEbH5E#(&DzywMo^ba6dD2>P9~hB6LbQ^0Jo35&zqi!Izo5VT7$d zWKEQBxP6cR$BsaD+-E2CwPihe?kwGr>iuTt^R=wCur-=BrwdBo+m2BH_nOK#bn?dH z7WcOD>GgT?c{1d5W zH+;1xX3M=mSB8j$qMM={#=8q|d`I$YzS<3CSD;=5zjXN@NznJuE!LxNdC+>@lEkD# zBVX(}h(l-!;EKBaI9Z1QVbm<@_q4Jo04ev}!*QRpAheboz~*cuqL@K^gkUemO*%W| zqyLSo`zIXyK3vK3t9|(E8c+VOv}g8^^0M@H^tWi;&{}c+5yarRuQ%f&Lnc?Vq`1`E zIuOP*DG)x48b|yqdm9cT^&y~KS|8(l`$Y~3Rl-*XTlSSEH9P#yM zN7_OYe=6NNCUBxX2qtYqYjJ*z1gI_x)sLWR3*!D-{C-3jp8PuvQ8xaRivV(@S_x0x z=gx2HQ+r4EWZJpHQX3@sblexjcmU|k@>dkCX150B); z?0X5%+Y19XjzA1_LlrGuDuwYxxE8ej!VQ`e4F@Q=p%uWwu@@tg5@`pQB7P#owxhK{ z^gp5Gii`hRtt;GNhYxg}MUH*+3G8eUBD@TuQFU zSH`--s0$7thrj!;k`4QbCbl06+SUNPdPd34rBZhmf(D@Q`3-cPpGpPpR`Ic;x>8mbQ1 zCoKus{yI(dgub+`yuVq$Jd{spsB0u!jLObZ4U`x3{W2vdj*{&AZPepHAXHo9r6!Ro z9(af-IZpx1P^_w)XEeJDB?%p73%Lpl!qA!DRan|6ip~$ut~QpgXf@0oGcsE89{Mmc zwinOOViJ~9&&`$cagyg)Zwd?Kocx;|RD^p<-9AoL^T7?0V)y(U@{b6Y}_y}G;f)80zC!5DEUx!x)R1_N8pQ)Q@}e=NH6?W zJ-@!Y!Z@$Hb6g2>ud)3Xt$n^bs7S;rpscqxQ`$y^o|Vy6L{uEs{90dyL@;PS%Y4BX>RP{+|pCws<*q9{(C8l)Sz>JNf4S}MVXZZ&bVqPp*cFaS=tZrg0x(aV=||LwB>!0 zg>VrI1UcE^1T!*2EeOP;mV>4aGJ^8)eRWu}=Y>8?n)ztDY zdf}(S2BGaO8c}MX?{INvp}Wdwzo+&5czwB6J%7V$qpTy+=3Gv7pOK6^bBhb6o~1Zl zrw}Y{DGp5JPmcsyr?CqHLKGl8RtHCZv=8Pmi`5}11^GAvVLNNd)?H;S&TsAQ9 zu`QGHTzJRO%nMh zTkLYDGYkP4KC>e3OX}K0O>SnQ=>yCk7XFh85f2I6El@QF+r2$lu&C>)gWS=Z=G%nS zKeF0Zp3Rrlec6?)|gAQ zuMM}Fw;X{{nY7LqZSIcQKH&(QBFMxa2KQ*Q8}`jg}SNOFm&rA#_~owlxI zdd3J$@8YsF1L}$yIdN4UIEpMb6gK>6`YAcGM-?bv3$LT%cxirw*N=b+mMvYmVoj;) zV=_g_29szL?bq2Ajy6Ir1<6yVHbQ>T`7cy!d$z6BwYl{@hbmyiyxq98Zji+4pLvl} zQj3Ge-w$EXyW~qDaWI9y`6B9X1K{ zS)U=0I!ziwZ&4s`M;Yit*Y9>4(~sMGLGl@VH$V5g#7Rzx)lSh3eKqvBC5@ZwjI1R5 z`*7w(SYEngJoUN!?Oc>dLmCU&Ogr(*(5#bM%6IfcD!v}h&~Agwe>rI?s(i}3e6@eB zm6kle*$O0o3|$+UjzLKrmm5nmkE{TBBj=CAiPm(4fSiw~uDm*21~U;bqloBOIhm*1SSk(9s?q6MFvK9k)@ zzYBOZT{>-tE6#jjZLb^q=hbqwf>p-Qp;XHjlX=9ksPVMf{oo)z;>go|G(q2q`iNfqMnvB zPwcLtD<-Dn{Z+9!rT)^gu*nTyY}+y`t35Lducbgk&jKDPkBOEFl{#SCQU4q&$-&1> z1GxCN+70$=xtnjuYrk37@5@bE6&lO&-P5Q7dSe7z-%@hYIwFGt=3hkyaiKM^Au~)& zvUzf$fg3W=-9vt)ycHoMrm-p_&>^OMPP8&;vWfDPu}RpuUnCN}VlnWtM_bEEI7u0- z)Mc^Ya96*hH^T`JAe?>lk1Ay&R@0*Y0K5jhRs`e(PX&KTT^K9I)X@~=%=}jU$XMI^ zs9sZ}EzoZpYc|hn%b3#}WI?Btl{1<*H&TsHjETv5>RMLNyjZ248FAy@xd}6v8uR9b zgBu=6eSfRh&W3AUBGaWH+-`lmAXxsU+r*`{`YkXRw&2B56QxIou&BujUMJVJ5ZFWQ zV-|UgJ4c4nG75r&3jOwU-c0;EQW5FM_H(kEIZ^1gqRDNSp^NEct>QtjAPv`*CcvF& z2>cW@IRU3#pH!8%dm#-(g0MfhB}n)OpI^4nJpAok4x>zwZzzJ~>se_4j z88dW@JxRX+4+i`J)HDXKMxnXN4TX#CW?9{RnpPUhX+>WH2jM_ixWM7en#ZrEP-b?& z96mL(kg?s*!NhrW!V{HpeE{CV*Oz39ofMQqiK(bo^6Q&O13LE(*W+n=OKct0WD8f?)~RAHKPg#dtr+;#lJ~C zS`OKkTj)7Zf_Z!GqCjNxpXFsaWOoXb;NB~%$#BvZz8r#HI8WI~3~C26u9#a}*|Z!< z(yni9W(lcN59ytX$Jyh%Lsbn+XA|&>$PK@ZhXlymSqXpire+wp`@zeh3>xGaPkIue z8O^e$aA|;PFEF&UP43taMsJh3cH!-#uG_9M|1R=adz$`+Qv}F%qrGz7cO6zX@4CIP z4R+l&z+P$+TV4u}qXm>!P~ZaUz-7_ArJ-PF*%_mYDI=WUZe%(`#h#3a7PKKE{UPKu z?uo^zzf8|Re@^D9NAUSFNDQtmp0bU|Vu=;*0mfW(ZS4kbqGumuNo>||P;Fc{uVfG? zotqjB$Y~mi<`uKA#<){PsvVWjEMKS~$}8YsIPwv3_H{_V@<#nk zHia&`{IXzuKFj+>e&hEfp^@{sk-``S_`{W{rqmvQ=OQPfA1(uW`RU-~C`_pnf5j62 zC;3N3VsDa4-qCKll(Cj*?wH zM!*;NzV=hBAv@pvB^}6gH`fZcr(G_t=T@Ro{2M_M~$n0Z+O9WF$I{$nQ`Xs*JnC=6wle@1zI{; z{roXFHYTNuPgbE+?RBWhts;YPqvarw%x`2tRax2An|kQ3m%FQa49-~<)tMwPju+0+ z=T4Vj4TUy>X{TwgH5?i#HQ!c;?H<<393Iwx>H&t{cFROB8k-ABzuA}XX(yh(X8cVo z|A!w;x9i{448V`W^XpwUa5&g;8j)1wZC8?E3`JUln#|b!U15UrKE}wEy+g+IYlM@vvUGnA{Pp!)9Kib zas!UYas76lv>U%9)*@CJ9Z0<8ylsxIy&~<_*;y?r{UXH_pv;u0YR<-hy@7|xvFE~%~~Zf&@eo^rLyF9XCCGAn-si|q;JYYFO!X^RSVN$fn0$(zd>S1 z%_-$WLv46$(H9G6#yX%vEhzppHV*hNt^ns;+ZmSP&IZ421sVzirCYO`PA)Cuq$$A( z0!kSSJr}td+c739R&?+)BD=MOQYZVY;S}@xc#2$ZE|2|7{93{F?V-W_cpS;|p`>x# zWozjQi8UyAGq3IaCWglU<#cWHqW2O%ZNs^D3cFuwOhL-r6hVm1XN1{&9B>JG1WI(d z^>~_V?2QNFb1F57wCZj?k80P7)Isy)^;2m?FWFAW_Av-uY%co+Yx}ODFZsy}82x!h z{4%{k6f{w0vD)VCvs#+hSzXrHa?IwVt7Pd==+xX);aVs}Ws8xE+k*pj>`&^xz z6}w>f!r%g$L>bz@!v%h->4@rTE5=l#*55GF8m!n1)QnhOIPvNTqYGj_w1B(!N@bNN zv4Y1ZTCqPv9#h?$f0`Dcn8-#77=}B>qUtOKWX0nu%G*XGemk;&5M6%$io!DK#*Hk? zhJ};jjKW|isn}|OZc#aurvxFriiz$}9oO%yK*JEVDdKL%eLRmfR_bWf;JG}Xzt&FL z^~babF983J+HqpMm>x_1&;V+V?^LZYuernfxtYqZYXR>RU7nBzKn+`^mU8KY1lfUt zS$69WC*4KACMw?d0iS)N2YhWU{myo}P>58QH5&1lBRUxZwVV&`h%47| zzMFs$^c3NeWacalLv?yH(ecXurj}Y-48DrM%+HfI7VwZgmRp{99qR{{Zwa1aEXZ@ILP5*GxX%oGyBEx_f8+b6=${plXI9h?Z^(*Gi`p zF)RVfsV>QIxKDXAC4;im$aloJUqWnSluf9bBU~)w;7~_tE{$#}#VX}>dC1=h>HAFM z;}llqF|CA2<7^c)@W-yjwu&Y_o+$1eQzjXip?p!0V{wr0p8(0I38vIlPaPn@doZz@KS)FE8T z_cH-M)qG&Fz?A9?W(w0!g3l)5V3L0MhN2`K^9?Nt+6d0kE13SRLOWLL@y6`Tv3H4m z1}@P<(^Lf_u95f)?ykOpT}hNqL>l2^|3HQV$wSE+U7+RXXaD!T*b9|(u zk+${Ww8`aQBSj&DwC}z-&-RBg}^iC=xJd_JXYt8w#6eWf?A#2FqmqPULx&Cx&T&Jk{Ch(Igy}1$=5~8l2k}|x&5sCcSc(3512-}T@F_Kiy0y;ms zO!=AI{Xn>3REu_eJrB$==AObsIlOAj-sI@nw}s>mw@5MYzwx7SDB~EMaji z@0G&9SJ`~2u{Y_WgZDA}PpR4CSrkBAOAlDRt=ZYVwIZ2&P8_+UspWbq&A>B)c#Z7< zf^7k2AXxFnq5zpAUcgG7zD)&tc2_;cTV1_Sh?Ikc_3z1Ag+^zSb)ZmpMfIk+pUHmb zcHY0d88PgKt*ALnfu954?mW+qBlYWW=^qBr)F5Pc#vuX%(VvgMq+@yce*m;;vM{Jj z(Zx#2Mc*>3YbqS}2rDP|C(ComTTBm!ZyPF`6jn}Ggzo|K@xXh7F+$Gb`-GJpqr06^ zs+1x&?OnTvjRU`)tVd*Y&%DR?d9asHwxmH!Y+>4{Dqp=?pUyxiSsyf?yTSBf5S!TX zJXqk}z#%cpHL;XYP-ndC7ML(%qy3#*Q;4x|ZI^hfHo;SJKvpLS_JzVwrvX!GC6|T>bBBCB3-Ce;SN_@kzs1=*vA>g%TOyZJa~?t zlJ0MsV}oUf=;QJdQJ+})78F`}@KoB^-a*Wd7if|xzF|jVSML9PMa`r`qAHq zez4Pq!L8TXoG<8o+)L=->7D&sT-HEVl4-fc)cbtr`IdO%|0(3FgZk*QZ36_i;O_1} z9D=*MySr=fMiL0_?(V_e9fG?AcX!uU-*@NE%)Rqoy;I#?-K*AKd+)PP|GUt2c|3Pn z7o(R!l!x;>g=WWvz7v*On0oHB)HZ{jzKi&LPH#9tmmwHZ`Npo}+;&(|d^;?_Z#ho? z>nTw?HS#U0Zf!LMxsRxPWw-jict+S||4ne-!b0#OCeVDKN#U^c$c|;n8soBC8{@qF zV&RG%253I8m~6O4*|e~qXXQdlXl}Mj;E>74jwW_1UZb`xmD3z3A)D zS2XX$r&n`eV;KAt4DCs=G;9G^QP6rr;%jd%60BDMWF~rQ-HPEdh8Li6pP(9T7Vn~lPIk1 z6FCr~H0M)a^M7uK?#s3SWD}y~+}`}3?+>S`wQzkwD}-x!d-?dK@IO*{VOXWUm@AX$ znoID=&Za(PEKlQ*^*;wKVc&hm>Z+6V0z^~lch9Cc@3MrLqGrge(ZBvgA|Z?n#UAqJ zNk8?x(L{?!sVt6G#;PONgsE*AEm*;=d+N(8Evcmqh;%TOolfrt_~)~`e=gtRmAJuv zSF~980maqZEt`JVIQx6`_OcB54G#!%w?a{SAY;1kt2=Z{Z%-L3)P$)RzKF^&Pv@aWCCF$M7Cy2R(;Y30j@jGJy? z+3=rF>miB-=w;8qfg zky}{#P z54>IFZ5gkw(*r>lKJ_n*wZ7wNP7mwrZ~S9F`-Hma0ciOdC4{b8I`l#h7{&2eaDHi| zIvR8n=+`*h*$K?a*y5iI*Sb!f^ow{wZ$W*C( zCIFRI$bhMWh-k<}#!+QTR zMsOLkvd2fii(W)_H6Ky|*@FQVZiAF_@@|;5*Lwk1?Ffluj zu^|G4W6rK6z=|~3L2x+lhml>+WJm^A$?5zPZfEPARy+3uiw-OD;LWcK^DP2y7aybS z&0kI&IBal-6F9eXQDsVtH+EurTm8Gw~iP?|rx!N~-| zPfF!)5{lLslz8*o8=F=!$A*@(O-Wh?%ztq{k%|j7F0S6PG7rnbP{N5(LgC1z(m#d3 z3NoSj+vkZfO+(Ku51Kwp3y`4;;Asnxp#!D;!Xa!Yu7|rC1rRes%KF>k1?E=tU-U@w zHH*q+enbSP{$ga_kqBeej>K4qK1nnav(Pthbbmlas2okxG#ojc;2>~!{&g4)X#E<) zk+3Rk4xru1iF`>#0F#4z$V0H7I`!4)s zY-oS;SoiaxEw=w--vPK+lU`q6m_=v{hbTwD<0pJeln3ViibcWwu~Y=xbJ0Vf}E=g4MC z*9`n<+?=JSTS-&hl%fWwtpbPes~jPOAO&(Y*fSCZGK76h0&z@OQxP-6hzkUAPlbN- zm)Rhy4Hgx8#m@6r1tRBe zy!W?wIFZsHxaRxPgyd}@qz{Jzpa^T!KB*6CYHih;w!oX}=8Qo~>k*%UQ!px?g46si z)L?}#BiCfneXn(92x3yT>-Ui~Ibq~2p4uB=FDe-`@*TTDGg3ae zm~M9dDC=m&)iNgatgv#8O-+{|39Lbi%t&)}Ktj{htw ztgY77AA^Ub>-Tp$U51K&$Qee(WAKTauyeAX=OUr!CSj`Mufx|^9P;s_ zqil)M#VjQ2vrWW+d4mErjpxsRZw+D{4QsWlNFyb zsh>;@6qN9XWK7hFwGCtx9CG5AF&kv0YidA+>zZL%I5N{fo$*-H7n3=e=-#CZ5-d`h zk8wtG#bouKAMTWR$m(0PUJ71*iRp3rX5O@7a6Ti7nqqGhx3p?b3qi)4{f#Rf4FxUk zz6_;>r+Q&|jb20Me-a{ndVhZBL&;AfU1>tk?^P|0K7!$2_+}8sajz6lEz-7WGUCBc z>FrD3zf1Q7U>lVwp`xE$IGUW)PInLlYe6)Q4mC%`+q&cIjp+Y2#Ig*iH*#5I+2;U5 zj1zl48g$R#@A0^+I>RiKDbCZVXFf)48?KE?E&g;kAPU~f^D z+YECS!>IEC$H0M9jT}=BY0(3**V48b^+CUC#9ZkD)O zSF+%z4PoPQwAB5xPP{73rlQbG{?+FZ-e6%73>=b#-&vP@abqjLY_uY__q*O?c z;Mqce-BDyoT@K2%P5w@{^QRbsfmze_P1{ zcO`eiN>+SK*3yEq)XZJoWcs(%gjJ`UivIA@QrufKpVr!B%i`+Rz2=A!lR`W~Tr9$q z*t7}vaQ1or@J<4`4uL=}n@rY;0i8Z$Wx}^`Z=beT084=ygRl-k_!g$ z)_~F>;bQ-MCaYT^j*2hZhG&}5Df$86d~Mr)`Hk}a^TOqW`HF|)$Y|7rO&vGyEUj_x zb6s!-wZ8D(vxfkSnG|EZkC{GzBdi}fTcZy3yT5Hg@axD)0fL6orxPyhFKW@LN_G3> zet}=tbWMM;a;BG+UgP(=a-9Y|`4v=AdRGiiptB&&RxwCyG8sW~V}d1o7E?0=h%M$p zrN+G0A^xw#NRtiwCHxM@$wlPFDoT8ELTGMAbY68}*rHZ|?-`xTJiKjO1Tf!G(l{1+FwXy9Mx3>)r5i zk#0XEP=rQo*rtho966|ZPxqG+cuC8|CSQjbV)A`&e+nWf$!3D|@%brMtHtK6>is5~ zlw-j7P8vc>}MDN8ZzOrYztE44hehSvJ`y zJ=o?u5L@yzf1g5PA1Iz8rdmCu*Z1;H?m|)PL&o;e*;GZ(lqYk?#U4fE*ZcL+hK#6Q zOKe8mOPZ+59U`%(zj~v}YVa_34vjmpbm#MHXP}sNV~2H3fZO&xNq5Ef0qX_;}H*a zT8#)Dw)Y;2tE+5%4}JFKLpKhY^&0q^ z#51a6Rr|Is1TZ+dJ~5Z^GFnJNRZiJfFl!_C5y{;5SB`4q-@8R}3>kmBV=k+7lKTVq z{#=Lb{Gs_i5IOnwZThCBym;;MHh+P0$MyRGiSf+g_ra@Trb&Cc#8_2e=e*{Bc zI|QYgHGrJ&AT$%%!xtlZDth6&4}EC?p6y+4;g##7t$to}-M<7))lEfeQE#VfskZ2H9AW!iL|qQvm^NDGRaa=skD2?+O(9eAJ_%KTh+OBz22uw$WWj9h;Kk7G~cSo*H}Q@J9HaG2MrAxW96 zkD=Ff>3@>Gbs_pPPDCDbLZws6V+Io7HULs{Ni1VF{nA7i-+iOB*cJVmlgUYAQUOpqii}nmzI?BiC#%3<51nA7R;#JXceC z{JMeSjjjYg%d-s)>`Ips%fM1wGStPj77rtzSby5%5zx^6vs&hSWJm<+`oLK=$b zBYxUZ$`2Q!mQDmOE1wzoO7Eup$!!k)Qy^9DvXvamVUGUPTONqJ9mZ=81=t-S>gizb zGJMb1gTR}Q3y9QjonxP~{2D&5Q$mEz08ul&632HIR2$&T6Kza9DgpoJ$x`qaC{dRH zUwO?{zJClRYjs;gU@QsU>|-@n$5q{`%T49j#U-1CRvHc77t%-Ufv=pSLPriW>v8-> zM2AxO<@IP);!(SvsMK;y9!M0KVKt^Av@ioLe&+BGOOKJ}S`ykOwz|}GW!4V-72KjH zi0nn!nqFyT{1pumb#~^ErL_%oBfrQ#H?3QC4F&;qz9MiQxH1T~`u;X*|2raTy!p#H z55)%#5$1hh&Z(pN8=Sp59;EvrAG`>I0{w5^=V@|d{wjKQUneu|c_4rvPOX7OHDwo` zE?&4WYoUR&4~}>j^JC-Rcx+|M1sh#Nun@V5cp^n^I(n z@KoP3jBox#G>8eLLDdYqB{mqiS#e40Yv-8X$?ajX&8&x^Y|DQ)g@tmB8vGIncYBER z92J4Z?E5Vv3UAX$81QjL_Cy~T=9LN1hntdHyP)NI*S|V-U`V>&F>rFSmI^D-WMgd5 zcTShKS1BznttU0XC_Z^H1C1+?&Rso&>u9tVD%7JPuMm2}U-Sf7&gAa}xYLMk%-0mQ zqHWZiQF6$cwng>uZpm8>t)BW#>9uMxe$nf2eoP>Phe>J81ZpY=Zn=^LO{F0ee&8K? zEZ#}yi{wx$98tHrY|T#!_?WFd33XYB9SlO=I3T&B15b7AU3y#=3+y`166k1FM57&yV?To>!bJ5DtA_E&OwYnqC z`wr2p0gA+Q-71f{A_?dX=8NDUcy(W&f%-qsY0*{sH)CjY8n&b3So-U{ey`(CZA2rP z%}1|2YgEk=4;S7$nVfzvgkR;b((J4-3VMmdtUqPbsp2UU`AZDB5gdY@lQKoZadU^< zr#Vy+6dQMIBq^ws$}|tlv`;=0_=fUNJ-8ML0=V@z*9bl*mf+z+t(>3clQWe3o`RE$ zBKc$RqLH@*I_kYo_N@S}>9?N676)gR+iUIWW2ET^=WUyB`CN#s0krbU0U%$iF(tZv zuu`?D#N!N$Ky%N;FJl{p z0m=nboHV_+XOhvK1V&O=C_Fn@;ygq>-eg=Xl=6jE+-hw*5<8|=uRR{#yhI13Tg7d^ zLn`C^ai;R?X|QQ%_Hvvhw=guURJ;;6hQJk5*C#QBL3-Jxf8bP_5}9je=jKi`8d5ob7%`-%X;*WqW-cW z*m!SOU-&RV$;%KlrFF4DXK-1MFO=5n$M~!>q$(>C4F#T;sce>p@VKIExp>!` zqc~ifWf{~f>{Sms`a$-Co?LZFO23{hPn@)rw2z!zP4u9%>0#Dcpv!TPX_%U>4B}~j~*GsMcSfRhzdmn}1d^WfSa6j0b*nV|vdy)vEiqBAk;1I74 zp7Q+%L?nYN!JVn}OK@wc-#KuQDdU~EaSlMSJ5usYY$X#c3;pI)QRf8;{+7w#4D(9Q zPm5YfLpw-<_jpR^NF92r0i`pDE=i70d^jF4g{eI0BLk+eEoqmg$TVVNj~o)ShF;?F zPN%B0epo6Dq>c7M2JgFWNo{?+!X3q9vC6@y2)u!b%(W@_uR2MDVX)l-O-A!G<$s@Ny`}5^(??Dz%xz+x9`MvK~z+x-<{0~0Y<$Lm^ zobPj?ghR@7_Kg0gxv)D>ov34>8n!A3Te-%MnXZxT9_@_(jYco(1jEbQ)*&KVNxpVl z><+Sa!m6_%OoSW{;SzuN3ObN01sN28Q|-Z{NNA+YmbVp5m)yb;C)m3flr7&YTXUOMx_$B2yb0a($4~u%17~tR)dzIQi+%5tCnvT-MCV%*eE0XXUu?IM2|aU*KO3y71lu+BSb$R7&jq8QTRcA|*K+=sSqa9P6IyQRPJ zc|*pM59n;B%pO^4)%Ejri|l*ThsDpukDxs3u2o!{oZFu1y)=DsI*9F53?#ag1q3fj z3~~j^siv44a0w+M^QyH2SdInV=xbHQs$aLfz+zM`05|~;!so-EwH13NLfrkLcmt4? zCanYmaOD(%Ii4l$Payc_fQ|Rlclxs20O~Lzn;hB^=hQqX49JM9EXCNIGRcPIpwPH7 zM;Lf^+y!aV(b|MNI50W-n%O|^pD>@aq{B3u`)~G$NIWo~ZfFUbnONFyCGswUpRi=Fe9PeTrUMTVyCJDLI#xcQ85Q z+aWRt#PBL_c$e}i9fkMQ#PS`Yl0}gzm7Y0tV6QUpuTJ8~;cnj=PwyIA_hGM5sZ7++ zd0}v9HZL?p!8{2eZXV@jun0wxa7rXimX=gU2u3++aRDSQ*~F8|6#_CL?J*CX;yeUS?-d?=3pe zYUlX;HxtjtI_2$%oU(-8eGCV3os|v|E5dDCB%I(>(@oS!waV@OqEaNrXp`(lu^Lh; z0OGhzK~{T+>5#g?f|#0n{wQMY-NjJc#a5H|$NQV^==&R0_EHzouMTG5d-26Ciz1lc zS}QnwT9vBh7jYP27bR~L8d_SZ1&R<#tA%VXE>h^AM=dH|tQd^DOQlS*yiC-_MGqG~4Ip*n&u>;aST_;bFr8&-;&q#_oHX z_d~JA30=Iy%Ew^O;=KxC`6sJ)fMY5L>%$o9-1A#{<>fQa$O+5Uj<$Eh-32}5Cm(YX z9mBUWH^NVe1;VAwxK8CBZbqdxVQWx$gRcu0J5i9KOgJxbKbaxaSqj06X3LUOd$z{HG|M zdH(vjeWu%7zievhk$X)$kFk9GPU%9~!(817v1~H-|NM4HytrCF`9pV#@gVK+h@OpM zAK?N_F9NzvlhXyus&K5OS2TyS zr;(Dy%GA^(4PRBP%6DSzZ=X0u=$U&NjmVg(+~Rj4&o9N{arsUm{UIZ@r3zw$h}Jgr zS1qd-nSybhq@41NDHLMib#~(2I_5xVi(-cvykBzIG@^VTTUYr2uL6hKR@))(mOoc= zM1CNy2guUy768avbFf?s%~*Kj@zIz0l{jG{;H+LLwV1~%KU2VcjNKhJW&D>&HLB4MQ2u2Oh6>lR7XOLg}5fBAy)WJv)hYr z1zx@8``3}gBogv?em7AmWtG0%*4ox}$|=rYO1G726SldvmbZqV@lfbyFtv!b$^%-5 zcda?F#5AG23aB9XdReB(jNBuvn#Z(uzGbkxLwWVPBZ4-0IJ=&BYzXTp)Sn%3XlaaP z)%uIFNdXQFjKt1up3Rq~Ql*9|Kaga)2G8u z%|WAJR+>_qcA(idd!6jH#_8&XbG5|0t10)OZzeFRyzm9qnb~&%bPwI8mp{L)YplQS*>n}CvLW{1Bd+L^&l*5y+S-NQK;Se$6n6)V!Kc`&Fm2b0n9TD)KHh|mM(6`6x z)dWagv6pxQWOE1y)Axm+qVw&BXDta{e}%jmmjJlOMa_5B9hkq|7;MuwH9xKNw}6|1 zwc{Og^!h=*`g}tEvBBmi2D~f0)>+c5?kGJAAYX#NO0Z35=ZbK_4zGl@bSE_YQ z%Z|@+dT`?Uj^%71oWi}6x`JNiZ#}lhcY_0|-~iP}btRqS24iLAn5aUmM0v(|Rx;L> zu+}gMT0es)t9hz<(NqW4j6`!h~SPJ#?CjfXd2($;Lx*rFa%+Q zi5!VTL*YvUE{5H?b7$TDBFj77{vw{85D-Xv7)S_Q2@cMN-MK!|e|@7NA#kbI{`GKY z(JXwMV>GW0xtG%!HjJD%=rLpp;$&XR@tyVS-~80=2b_^WL2IQGARyJ8wD)d;$_2VN zv3VY%5#*~m3_#8P4Np`-;Y%|j0X>EF9ri5EVLf69=OQ#p7a~TfcN+oA9k=3eHtugQ zP<0m5L;1yuaPxtIq8~b1aB{*oQ?^S{P%a0`kCmb}M*O+#eTEL)-Srt@cQ;z$osPTo z^)Q_@02|oxe(w|AG+ZMmUSnPEoi(+E&}iMKEaU@5z1ev}4F!WDX-&%@+$!8ldHulR9B%h8L@ zbAzutZse;sa-1Z{9X`)w_MkKBC{>jnn6_7;P`@X8kg+`Rj^JKcczPb5J1^}Xk>v&p zv<*r_1K}A5?e)c2_c-hgY)h$aqA_HGV1)ua~S#FZE}#&3|ZvW3?jo zyP{#e21%jR1Ye|=l%-qxj~US>X!C;g6(>%0d2xOcd|(d!*QP4Ck8V`zf zz62}IDD{sUmL(S|r7M1KFU6GO>6y44yE7DYi;S6>xQ)9X_1_v4l=LiKGh8@N^Mo+V z1bb^Ah^xcObILIxf9TaFkn=!^T5HKbzWX}ur@XTtQ6xR z@9R?O39@?O~S zmr%IP9q`~a+U?;z3qmXjX z%uc77PW5K;jqj!8uCJLD1b> zFVElvCzg2gyWpD63@SyrC^1{08RehX2e9^2Lzm!U1*OIO9HXrx5KDq*v?~4jw`VrT z&L+Nreq)Q>A6pWgVmqeYa}2(SytS8xYxG^_RIw9XA|RLY4R+lm+{F9i>qFyBHf+of z#O1 zU^l@*5KF`U+rb;Ko~R&y_-=$t4n_ZfX0rqcSVQn;3R3CB?ca{`I7>OafvwwXmNm-q z`vH)GBmk``*%TzgCyM~s9Kq}FRA3pO&Hbaz`l}Kw^jD&4k}a~5X}!tM;|G4Koqhz< zn|EP0-gdqvA@Lhcr!lNF5+t?duFc?a=(N^PSip3Y4S%ChEJ`qn+qOH$CO7LM8Aw1) zY_F&wqwG!yY#*vhFt_3B48<7`FX=@OGpS(IRRS!FO8T)h2QZg-X((u4uwI74s-tUd zq7q^&hUa-$0e$h{Kl=H940M5zd%Wi`F`6tUT~;g0+)5 ze*<(k;#R3lL+=?aD$y8wJ+XsftpycSm8NaA{3=ay<1`qle9Y`a?^%2LcYEFH?vW{< z`Xqnl9@u<`SnDPRL7{iIBAt9?_QwiH21Dzn4RQs~7zc9zTO3jEMs|fjx_5_K+g+NE zDAq$vs1c~yh;=r7jXr%)?iyH+c}6~MQUFs;XauE=x6Foq} ztJhIY?0h=ZRk4=C;l?N9nUfMpCbKgBZ)>Z2x*H`IaVC)}mmX7dJ)k|Op9>*F%Y(G4?wc-mt1{iceVB? zQId$%wsu_Eovb+?L;F*B97Buug$f|JU02uj4bx(0(T-#{h_vZN*i|zWN_r)}y2E@z zX_`8zOJAvQ7O7)G@yHm@+onhAqi=y-a`ZQ^i7=lYrpKiYwuON5r|age?(WCJ=3Mqd zc&$hnh=~-$1AWT)ydMD0o#E*pE9&cMf+Jjyx1=*o?WH_}u^3o(X z;CB1cZ`W6r-xeCOAap_6N`FDl7&684GrR-pY_u6EPupp*PAjv>Mzk{gm|#(04q1;I z>L&Y_%<+cDggEQ5!ItM)o)<9grtMqDeBggjOC^z8)KVbd;$B)cDRoTqdBS22O;}I0 zYwuV!+ik?(95QP`=^Cq?FH7Wj$CBv~SNZMhN_$CTOP2bBW<_S+gqRy$@Mgo99JXzT zjJx(u5~5!NjfBK&`q{y$?>*y7@)`e&xp#wJgT^x5nGHc9Qn5+oju9|K8`7t=SpL_d&RD3NPE61RtQpFdJH-U6Iz2aR(?okeO4GM?2vwHH|b<4}_tg$FkLa$Yp~t z%X&#}ytl#rsNAYNJ4rZ(`H`wNJ4jm+=T9OJDOnf z_7d2Ke_P+4&*k@M5Dw7LlsqP{`c}~2vn`AkAg*t2B7fiL`ClAoHHVo6W zVjz4>X1DqF6g7Bvu>C!G9kk#V(x$1b>#Y(bDrFsy8JnXGLEol zDdf^jIZUPVd;y@GV{Ij+ZIT~d9G<10KG^ighIbX_Q=%eR@U(qnt2{ipRW{&hy_nW) zva}63EU!Bv+@eG|NB3n9?MxFj$b6Sy#)`FRZ*!OtTIq|v)A>58Ue)VGP*H)@l*~P( zy|-;w`io16J5qzLDsFzg`pHqQP3!Z|sif{Cf0(Td)Lejl8}{}|aUeA5__x)wHUIAS zwt}Ke4;3puIohwrLMIYKP#UxhsPq5*F zb)M5rUTzZhTVZ>){&iYh7-pf=KHlG}szC~`uU85O4X;0_L|$84H{?`Ak?D$f=0+W9 z4#x$Ky>U6K3j5E5kWB)=-YNUmM7~yIWrrP77*B*t7T&1yW0kfyTIYpzrok&m_| z)S%;%e?T^ffif8O&kMT5Bo8LA%D31p2CsiX=_{6GX)CW{MdGMT&`MF`Oezd z))XF!Rj&oM7JYAj4{NFv5_|;+(ies(GkhCq!?Y{i+rHl9zHn$@&~s<4Oi~u9W-5p0 zvA7wNwU=~L?7&|J(qDyfR~gEz2NGxnCGR3WrZ!M`j6bqI{-8f`80G;klb+&kXl>yx zqnwg8vBpB`in<0m)7~)~(%vz%{&9O-J?n5;KI>#C&2IE)&2Efz-hPWy0O+`Hn`A(5 zk)XFskC~00@u`hXKZG}#3X{VNM|}~1?CJKKBWqpP+)P*hv-y3~=CVP`yE$SJK*ReY z!|;~be{`|wAb;rK|3TpVH-dw`D={-iJdcAQ z8Z+oGCX=$6i-Vi9iJ1#AGiQ9HgYW+XSBkfD{1RX6gpS9gXyk16m(2e)UP+qkFWG-D z@GsyWcEKVJZuYLktW4ro&MvOR%-nxhAZz5}O3d{V$55vzhyU%l;SrZ`psRGykOu$I1`KBx>g3s>2!Y1f6zV&rOM>tMm8Xk_u% z?tlvarT~bB={%XJcn)jYo9BBW8mG{c{m>u(PwW6PpwN7skrT%FY%q z>B0%g%E`vf761M3Hx4$I_-+?6fSHYrjf?p2SYrGCj^JYXYyN+&e`3sRY#iMG3uEPA z=K9|}Rt}cGX8b>5SvlB1%>RLL|78gc{7)h$3kS#lCUUZH|3_j@POkstaj~)dM=Uo7 z>;J~MK-~X|CH%)8aj~-eM-nbJ*7zz{e1MHf*}=h;`0r6E!!b$On>!G*{NpE+5+&9l z=4EB$<`!Y)3I4h(!#6vW~F4~f61kpKVy delta 22794 zcmV(iK=;4)vI6|-0kCWr4lhh)ZfA68F(5NEHaRwvzc?9x+q4kJE*6hWt%gWXMCPL_S zgs^Y+>JC$X`>60R!0;2N%w0Bbu{LYXB!uKz*k3Yl(b~D*+_32gc^*gTveEO~TIU?S zB^3OZKyJb(A7`VeAdEuWtDKfnX>ciN@lIg2E!RxFlJcN|%*aRA#FB z^{+<%zxxP#Q51eL;VBg5(M0=+`KWMxqDMI`nF#@8f8$3ss{>fR-LX2cYex=vuTF zg>WSI8M+SL#JvG$-Gtnz1f7p2prz_QgYWj(a~AzFd%=bCw(o0t0r zdQgn6gg)?SEINSqkqlhD4Sk9ucpWzauJTx}Gq)dFB~UY(kM2i%aUH&Z6bmzRV{->k z2=1{4u6iHZfp){U8~qf$hc)4N?(y7l6h&3&B6yxI^gFzd%l2%@Hc~_bmMTy^99fEg zeujR94&fyJIaw-b!XUvCuFAcM0%#zb3cY;_&in-blUxhm>$q3>(YfKs19-WUVjKMu z{Tat_H6D+rkqWYuJjks;GTd<>eCMEc!1ujy^$&0c?jlD|RNDBLO@%}&qm&;CC9&)lHg zMl=!VVFUF3ZuB5L*&cKdy$jzD(MMRoF6@D?iHq@6d^LQp#kb){@zeMj+y!@kIfOsL zpW(0YKkzAn;46|iDJCWGO_CMlO7ea3067TXL*xtcFD}fLa2c+SYv7u=rO>NQ+*bJR z;{MFX_=9{7@H|M^COjlOE&ND$MK~^M()BWuUqAg=Pi4;sS(LpwyDhsT+m-t>3c?u1 z01HK^0easG-vuz<+koPpM{i(%4R9C3mH0e-31H?5yZ|r9YXFls;`{LvtnWX;F9DX` z!GD83yGerexj(5R!^wE~UQXJ`a>fUJdJNA z*WfNvDy$LDA?M(6=s2GST)#>lA}7f?+*mvYPeu#KK>KQ9fPWS?4g4?AG5#fZw%@^( z*N7Uvmi$fB&<>19J>2tum)rn8!@Y*y<37X^{|Ndcui`L1MxNp(02Ta{KTnvBin#~S zPq^jy8nlayLP$L&Zvi?Uho1$xFa;07UvoLck#RspL%6@78_+`XU+5U{@y+NSJcpl$ zZbvnE9r_eK30zSjEDId>*Q*Iqx@oDrix&u9pZ^~YcmZ3u6oDc9N!f0|(7@g}! zHj{VBWU}q-@da$Ba0Go0-=DzFdBTfmGye{nj2d%Wa&G~3mxF}856wasqa*N?U&0+O z^QPLH=0`p&-)>on0qQ$gjF;@w+M|#FQF$S0kukh85rYE{0Dg6t5F-7lUD7HJ*2F!#Tt1hYqQ$tr;|MKz09qRhi0)^0IWQG+9z? z78MpG;<0EX90~^fK126<+?q>OWJwfwj$l+ZDml8v>`b?G^6BJ-7xtt3$yTUn?Niax zX+qWLv)goknk}rEd3H++TF(7eOUrI)^|r*i*?=1QRhgrbX6J#CNwXWzm^dBa+eRjv z%+6yB9?RgZ40Z!tTns0eqayQ1nw_}C9Mw5`_597FT1LXiy461$DrPM-$dYHqLEw|Psqj%KxFv^mK+tuv>0a;;7D z9!3W4JF+u;)se`VUAVY!#Pm&lkH)#pqay7l-QB!-liB(3#OZyH7Spn+39bR>lGNyy z&7{CHV{OD&lk+z(Xn~=LZSF*q z))w!G#jHKK!zeb&+&pD^vbeJ`o@{Cznb;mcnBoYgr8Mx?z{ z8PT#?A4cox`JF;aPnw(mfq(*%$G$kbrZrz9ru2Ux3Q{`hB{ewiz@3>)XJsWN5orXB zDfIC?wqMt;YIQeBCYR|ZYyf>|0^qi_X;?L&zqptV@U7hzngu(Z>nBdP_f0e_z5`i* z)tRPF(n62ybB+Y3(j)7gBfY1$B!QBFCz=YJnS#IW+`ggo*A(xx0A(X+1E4>%YHj2@+1(hiI@zhiXm&TT>vX4%Kpk(_S*(tJsEipgWqM!g zVJz9y52%q~reF&QGo#! z?{Pol?e-q^eCpLbnr0f_fMIwIucr7w8;*rk5oS-fCPWk^6plp;!?edP*YK$nu!1g^ zI5iwb#U+I>lqeGMdOdPscP=c83uf9sr2foIeE_0k<2*#M8^I!H;nCpO@rQilI}*2M2^7*H8$w*ZEWai z@YPpmGCi@esl`U_7y;mP*_=Z7M6J8FL zdypshffb9^8zs?tAN)L4qFyfvz^@2?!TS6~nre*VK_O6GL@5*wh5bpcKPgKmlLk}| z9$Z(OP8L5xHt&D^s^7jbwtVU(xf8EUU2mlYI+LRpq1#PM_-Ri)5{WQh)N zweG8xrf?UVs_7y;q zj?l5_aYmv+?`aq~05B9R&i@|eOHV(@Wlq1v-6-sRF5CEj68gZ3 zimcxfD@BR+UScKg&i#wgDD>uEmO&v=ppPZ75)Pm0q5vE|Wf>F_NFk_@h^dlFH(9yU zfSTC?<$MTflG5#s&9t&wcZExc?)$PlXb|Kmco&n)XiT zr^~;Q`4Ao0kUv;k%bz2U<}a34d!H1JdL<1Jqno_ALsS9|2L~Ju2H@mw`t*S`>CSz@ z#u$LFEEjzYqSL1$l#BaQrX$T}2%Dh^A<`0B7Fr*F;zDkQcsC;(HzON&+ElShyP`r@ zR7O6kLq4iQK58#Jt3183>aerg%g$^oL(ZeGq%IM(LoV~q`rTTwHv zF#?27mXwk@1He$IX3$`RZ+T@-_SBod%YL)$l?$G`=I!0W-qYLP&z^qlcI^I)8-IGo z%e!WOy@CUP)dxVv7Xelk{L*eNLZ3^9f!;*{pUZh5IiiKgRE|ef=iB ze`eD)`?rg9a@~Wt+~F5AqgFPd06Ez-a6l%5GawhcWNUOiJomocqw8gBkPQy1mq0=S zQGKvG3g93cq_s%~TCQZhRyCIFO0L!g)L34bOF&4R&FQu^T;r(F_g(3 zx54QD%PtDQ(0pl?Q%JLi%Byh-!bDbF9waM7b%}HkTslQOP|P>G*@UkHsz2$<56{=m z@SL`X2PUA*0akzj*tbtVbZDOeb5SOLV@m=Qw~=fWNhTdWk(C@Pc~%OnlqnS@8INkL zg-FMkcxVYUM;VKi672!gR_O2-Q7A27%~XB0URDSihp-2x2^r=K`rvu){L<#m7s)i_ zLpqsexe>b&5*=c)#T=&3a-#YKn`avv8tjK@?jzUOKxN|Ab%=Q70Ex@|YHg!`_G>_; zc9C|Gm#g4Y?kdl8?lOM0dyQw4TXqpau6Gaij3;Bbk&-2kbr1Kb_mca#ZPGURY3?aW z^bxPeGe95#K_If`b`KC_0A+2GcM`TRAu{#U1CMz;IvuAL-+CYM?IlkmHy*e{Fy(GM z&V$-G1!as**(&XA|U#iPU3uH`mtl11jb{V`bX$y-~cqH z(=~ZaXCdgZKSf&Z6j15gh~(ZLI&^3g9@DA8v2r-J2f2YcJSweanu$Aq_vY^6dQVkx zJ?Nru?jBO_88n1}yZS+GzdRQ;tytcSmN(<(CYyzdaVR`^2rf2~1_n;L7nkD8285z@ z_zEn%n4R`~b~-TCS9e}G;RoDlV64|p)$xZuo)lt3!;ZN` zc4Ic2f-N&W?D`Cr7>qq$-JbEf@_|-SH*q$ zaor^gO?VoapwD-8>VMb%?*6++;WggPdpMV>2t2R3J+dfC8o;uDs7YXU&?ojXp=?ST z%pDrcBD5|@>o}9w0&td62sB(Ma$+}GW+_P4KC?iHBYUw6VeGPenu*#ZZqfw)AparH zZMC%(%QZpUCw-`KTQ#iFecdY^l*o0`dWlHi^S=EK2>KJv%cJmvzsND02#)~`HpGrK z9s#0+KT3NU(1Iy{@zE_dK=4z!ukYXQ*}s32U~hq`&k$u7@8x7^Z|*okxvz&Zssx(K z{12ZbuE9yJnDZBN=`vB`NX_rb^!I<%^Mgm;#s7WZ=#oT@u=ktM_@(SfG6QehbLDNf zQvG%tXn&PJUjn+`(lFt)F{HQk*DQjFFq6wzrv zqpXav(oO9r+HK7NTn>VaaZ611SecJ3twggQF=_?oOAI zQm!mD*jt+%nHp6+!^OnNpB53zF4uXdnV>= zwNJ@^QxiaGRDmnHbDwv4JY;GG?RF8pW<_jJu}04+W<*;o)5d@Soc2*Akv8@@)avV% z9KI=(sG|dC+Xhzi*dsujz|K&PqXgT`)Z-J!7&7#p=FAaVoS;|$JC`yv1`u>L)V%x* zPvX8iY^wGTDI_(vby}7#Ela2CY6lOl0V_6tETz-Q5-}JEhxjlvYDG}J(=%UiU-9c} zmOeXq!pw8Bizc?uyY{Q^KlZPU!d~xl&vZUgKNP<^ef?D%PyO&$*}vb1-_e)ccIohy zBS+0khFdd39&20r^EvIWZ}8lD`-aQL*VHU5KWEqKRR>pgd`3CzWhgcP=X1EkiV~ZD z%~?s9#mD_8yN@u7kNXb0kBJ8iHXi{Z+P8D)xwULtQ)_Sg4Y0T674|lnvbP0=_BIk@ zTdUHo*P6ms;dudU0g&!@podW>;;WH`CZG?|aU}RmsN9M;)|@H(D3TXPUpV6EOGg}? zv~+uFVEWag{M&t$bLxWF0ZJ@P)XP_YH1zb!S86_Hq z;Hc#@i|_~;2p?t>Lof6qIqcYlVTWqNv>PQ9J>j&e;3A9mO<^KbQE7*j2&a!<5`DbMz^-6-R{=V>kCi0*-4q0PjUL4Q^fp|C`=XsqExWcYD|4ewd=T#3E z4oBj|EU=96{_$ZeHeHymT&hnsuJF$YUlExWyEJ}l_&%aX3pvhL=u(2tpaq>l3(`Sj zG#yNv5|*eeVoDL6J?Icf4wV6LoMB#~;)v22Ys?&&RO4!7wnA)u0WR==G8Xf)q4lzh zd((7GWn0l=yd?Iq1fq%AGkdLeTL_KIJK%aNm`tKZ`*PF_*1TUw#e;Z?c4oW}(NWDH zWYiK6FK9Nt84rF9kN#0tcK6E%vwNTZH7evpav6eTV!OFUCK7CHusmKFmJ6 z>(_Y3&$9o_9>ldcz7xBDzL)(N`Y8hKj|SS;2(dumQ`*#6$wYy>*9Al&TVvE%iEZK> z-fKaF^2z71d77bBrY-U!{wpiYWRRCl+e{K^-yx4|O;#Dk@#nxvpYHU9YvC%NS{?xS zC=?tm6zcJf6w^chWt{^6Uk-&Y%~!~kYPE--kLQc?T_1?Nz;m2`C`*bcDk7(-8s$z? zbp^nX0ku)#C_RK|EoTxOAOwq=OT-*O*wsy<78pDVM?m~}x=F-RG-Z;du2;eA!@B@g zF3>NzN#n^Kgb-SzV2lC|BU>)U$y%O`KgzQ)iR^ZJUMX&Q8uZ4Kw)5abGt55r<`e4U z0!!xvvkI}VeiNvFnKI~?0#h_8w27*hIuv6%!=P9usA4H=ipKBFoj_df1ZJ8gowP9X z#wtolv|ffE7zQ6~k5X%<>C9yIZz^Z80XiqMY-+GU&gpsW3tT*5)bPu3;-j7y$YO46 zcJ#V+E4Sk3Pw(vc9-VWd2uZ7{-=D18gS@${O$hen<8df|#M_F82py6@76ey@D5m*f?y8Xd=?nP9C@tBn*!YGZ;^gelr(!8yVl zZDFuO=+Lfz4tj+kHT``ulD$NxUjIh=p(EAI^#8b3$njuci;^r0Dm<>@_ISNo!0+?X z+_wlwpN5?RikNh(`3$6M2?*^wy*7d_6DeElLzb{ zC>r)VP~QSP7Dx92@jvs1GK3mO4k;}y;5I|#QJ z*Z6IJvi195;|IeDQ=19n2eViI>ci65P!)$i|HJrXqTeUK$S!#?`&yY44rG5L>^M-;Iyo&l zH@Q-|Q4!l?tAu6BO4kj-4KA@Pq;Qe4%EC~8fkG#iytaGP5rs#sIMa3&zpt>evZ4an znnqD!A&gwanY1F#q(u>Axmf@xEoxN0h~2qQtP~3*`B)T5WDz8p`M;Q=F9ujNX-dku zTFSW^HBT%*y;>@*CFs>a2XNcrH>-?ZL5o#Ee^?lc@nIpG(+dADf*s$Eab&(jQDG{7 zxb2~K+}CI6Ecti`jXz}!N_JEkr!yj4u1}B%O3*}b@gQ3f0X+nc4LOh4@HUcu`n8pF z=iPM2rR#sbCHp;m?uMZkj~RXagV{gg#h0f?%osN1?k(BpguP9B+Ae>xrtGEl^R~AP zqRi(I<%BVY5S>dj%talF% z4v7z|yr{C--CViA-Co%;V6%H;#r>fl#GY{n%k9AzsU>*Sic;vw=(FX!qc4{4j~*=l zL-74_d1MF|(jhP?@_fECE|oeOTo_NGqHs|pQ&m}8&(~L7#9vr7O>WA}mD@9atF=wq zZ?u28|CKR@)Os+lSC`g?2Nee*S5z#mAQg#fPow7!&qJP^M|jBdyytHo&eQ8O@@RA% zJyf33k);yWD%N!o4$!glh+Z$0X-cmi=JEG<5?r{OJZrflRg60C4kQv1>g_rjRi+L~ zaIT70y|u3=@M}j^IBj{DP9ZXXx=9t6QVlO^EC{tXK{hTWbJ_Zef8ww zdR!l-53rD4I?UW^;Z&rgn%SAvZ0f0ItWaJ0vUpG=MPj2!!~k=u1!x<8(RRyu7)!D# zUyLx8)M)=j<_8u%j3sr(o&$&W1`{YtEVnJuOhyOm@+8yWdt*9`T{0hkOjW`onZ{#1 znIi@Z%l4kT+&1FtnE_8{em1pJ*AiNuqBbY9NQba*U2Pe)Nu{#$2s2Sb!C)X1PNq3g z@(=)Ps7hbQHO$$w;Q5z-E?9YC-NN_g;hIr5U$?fPGqU8+EjK?qL07^hFD1gW_Ai|| zXmR`eN7Ds2Odb8Bo5pPz7x1`ar73kuzjK?GN0#3@#%jH||C-~cZaQ};e!o1SmyfN! zu;sGx=Uz#<>1I$K8mO&`B>WUutLmBQKkuki^V;EQhlFfEZ~&fv(ozA_7no|nu~KBE zsX=e6I{j`nK+_h_eb$qXWC~`QGi}0o#@B~7> zgdkxtaw?i{DrVy+0yMTJm=2n|jZn7sqK5 zn28Rd!-z-Y!urX+&L(@_Jkh{pMDy9Bbc|UNECktj%%IvDv-v}!p{TMdfsL=H4bcku+*;r&aISGy_~UbylpK7G&gqn3>w zbWP7nvT@1c^Y41S=cW9}i)k47B>89$^5=(5cgD}>0EPn;I82mqfVcz19H77ng(Msx z?f@|d&>YRi?EoGJ@H&9sPCLl=hlQyfz;FP+Lvy+V_#D7+0JmfM%ML&dP-`p@bc#R1 zA5s2)9RAo8-V#olBrKarB@#Ckj!PCM#9#tO9q2n5i|Xp36yBP8I7L#RoO@DR4Q%j~ zUW^ExHViiVQK~TllxhsBDugNJ7=%)ep;0O}*t};rN|xb7V~lRxY!yc2J}E;?h(%Ic z<2cT45bwP~oZTQ!W3>joL7b6hT%|XF+N{lgCUK44D(+0Nae9YxL~4@k@=3-ENyY`q z6h4ITR`f6;MW_*t2boT4X%a%lJ55~J7)O`M6GF%abxvk`Z0qhF!}29tlEFI8) zj5zkWgL#(Nz_iQnGK=#c#wwIo!nEkiEffNP^35ZIo?Ni{o}z1i`{1)XlQYj-_WiEu zb1vC1j8ES^?uuE{_ddV7r;Pk?(G|n)e!S-%vSZDf3HRUG^DfMYBSF6`15R?ID1Lc& zFhcR*r;TW2(y2X#>gwd$jigX%cdg6 zGbqI9%7WWb@+8zJ!yI#(69+{DW3@x_v*Ve793?7!d8YYfrnyg)uCI5FMueGAEt zZ7q`elTvs8&@fWx9z#aCM+YuST&hlU&vSn)ej54)pYZ57$a!447sV4U$v|olH2R1m z4k_>glQcDS`upkyhQpNMbsF?~WxUkySH>JYDP2EgVBN5c7Gu4^7cmO5z2=6Gi9u#U zFi|KnG014jV97sB0D72z0CW;9`iMTD&K4%qJA8~#d~9faj8J@~5(^#DtkdCBUzQF^ zA4)liFOnLi@j!HijAEItE)_BwX1cn>j2|h+Xf;|`JE2dO6`gsR+1k@bFf=c3V1XsQ zC)02wpL0cj%yMPUI7%zUbyOJ#ZQRFX2xwBc4`pye+xB1g)~W@6Z{EUz_iRTl~H=~K~_22&d)%RZCPqpYVATtU%=fPFP2gmVALttv9hf-ZH#T;DVxn(m zbW)<#w>Z|CSR<}~37#Y;BRVK{UUxV=Aw<*KxKP5oRex9~y3WTFs)Y8EXAuT-iw+}; z1hS#hF~)cM6TB;Iv;En@ldqX67Peq+{S$K`xNXls*o7-(^R3}aZj*uXJSK2 zun`Zj(T8y#a&~rI#-4eg4rt&c)4&{WJqpoS#v|r@&=|+`y77-wZ>N9Fp~v_ z?Yj?1#Vq>{7pIv)#$CR*>dQT!W&ef)e|igh@ad!Kj+3NS#)P_#GW!*D)TKKUp zT!cBS;qvT%2ibq==JR{!R6_JYSdGk-XDTz*nXZ}IO!oqLfwDkd;98)6EpT_GyUM&Ya9Ub1xO9fvC`i%Bi+2iFq)4xoI${j7N!~v2HQ0f5r=M|kt5S@n- zoyQcZn)!j{E3BU(m!&k7kD2Kp@9JL=quz~@XcZGs(Z=Zb=oQiDqX(m+H(C^38vQWJ z7e()Xh?3|}VX_GV6EfPj0<^hKlc{uk2or>LOeinx41{VKPv{<_7UTXi3lDuylwzYHO1)8&o*6aiSyAR& zh%)M>M`7&ulFN{k`)W5cs7ov1Lc0?6hbnP@CA}v-ztTxbue3dtA_YEYD6FLZ5_*-& z80$eXaAM1#eS=8jp!I{uAj;mQC}OL3Y_>IRn)6w$t!j0(+CFVhjctnXfv zHuh4JC{4TIWyyG6CVRas#N;jc5MioO5)>Vn=f>vcWBWP`GdgTnjB^|(napy^lYNwb z!DFBW(uR%4mNT~rXpk(@PdB#XB=0u?rO+ztSC|w6RcS-_>3*FPOWbB0Ddkce3;m!d z48U%&CmBa2Nw+3fsBv6grl?|ukE0^JfU1!h>M*wprbbp~Hf-2{`b-qm!frlOK^ysp zgzRacEM3;0)Ya0c>bDXI;9E#+bt2t=xWjwP)$7*OrM~y-edEs`T6yQ>YkoSz=+suW zuUilbRmX39`JQR*uU>QTU3_k0;fl7A=O!bmK^JWpcfs27qRfR?&x=f&IcZ2TQQ%ig zYtCOcbH+oL{ut%sfG_ZAfgk1vGVLT?n?*gfLfsMjv;{ z=XS7AXJaUedH?q}j+K@V7TQDXa}LP82P83X;N@e6b~s390V0vlJJC;{@ei!a!ln>uVy zrkKy#ezQw+6Z>i|3K^zmAK2G_<*_|D`>bjT85Y}D4Gtqslte6gRiwH#=FHa&On8+y z)VwYDHDfFT==Z<@{p|z#n=BnRAASM_;|wvyt&lf>EBOkQTx?us+-`96d02|-VaF?U z*ii?LTS`%Jt)3_-qcPRv)(b_YwY;b)ele~@eFBeo(WSUNvQI~TE+8d;Accr{hzL&d4dkQ~Io_0N{?Lxc7y`Eq5Z;S6LNBN`PPkkrE zZB#M{;oI?209 z3RhA9yK%rTsxBj~W{fHPBz2~-$XI7=HdI69IfUtO*aP$JNMCj4M75nMq#vPQTaLhA z+zN0k0w@WJs>(pls%}ulc+5@#`9K_AWX)B*9`hH5B%6}q^JN5oDIf@v2gW(&_5|E+ zj|@6+MwJ6_It>e^?64prSn~0**U&s});*tF)9A_-8ngC!y&exz11EJiZgDSjuXl6q zZv2#`n&VZxRK1QytjSbM8E@dF#&rf=>@d}G=>l#Mmf6X;_^Dm^r2phxCSjvvPc%12 zKtjS_EZPI`cZURjZ5@Y(1AYIih@j-r8|aV1^f#unX!7(fx2c)rrQBhdMGqrS?obyR z;5C7d53@)%i)N4MtewoV&JS&u24GfEJb6rK4T}}Zxx?Ef)2{X9Q!VLoqeHu4et~O) zBsjD~8bGhO0}Um6?K|~ey!X5?JI}}+-l>|riH7FGjrqlYOK)uSr-0fTS%pNXQ= zX~a$gW4Xa+%k}^Cp}|TPr1Xbbq>?M+@R;n2d!K3KYo6KjP~Ew^pU-x^_)Nt+e7ffc zM~vT+B|Z1Pc7V)1^&VNb>-0g8)up+wNTqNe3ge`Q4lC&{CfQ7zIkX}z>n z;t`nd4@;fWKIxDovILrZCd~<^SV&aqrv{zmNHNKd1~K3prYBIx08N+>nd}$sd>p2P z?v$uM{+5#qjtf|p|(`Rv7Ds0sT-T2kr^8bE4{Yi!Ae6uGeuanp6k5W>%uh2Z5iSa?*n5x0tONG1SOfFM6+#9 zh78+(d%^-c2i|kzGjPQ>LPxem^xA|bc?#}4x{5p zL0+#5o#|r&x_*`!?_;xUDfPvG@GXh6RbD55obbw*)&}MUD0O9uKwN=tXn8Wjr(%nR zQY;pY8O#DY78AV|9d>*n9c&zk#pvvP4hGftRjrwZ6`3U^6WKZ0@yeZFr zzK+qBx7ZzhjB0CK!}fx|{p7vuKUaKq%X5D!dOmvHjGLc*{Kf^h9&>^HGwcY*pIbdr$}%F&M7pXn2=-t5SR&H;UWq=2=xR9IXw@hGxKo zf{`}?^Mp&)91n};Skg=}t7Q68N@0P235#-ASXdw4{kyR?A#>uFFWxi{5AG` z`CiYdLrt5vz4+5?QI;lU+yv^ut01Ec^no?D+K+V}C;3`_1V5Rd%XjdiV#tcDxc!FW zMx2aY2^x|>s#3mH#&U`2$9_^`urV{(C>m@OjsHA;U-vH7vW!0CC$jN>JIk|g522m- zDBDBmyI}u!4I!}Ki4{kvrgh9v(n8@FY7K>AB41IaQU!VrOoQq3Dy*|+;`YoT=bXl6| zW!AelTAhi#AB!A_MRmGiPt26~_)c$v3f&K^#fkdzz%=jkDrdPZFY%h?18Q|zNSfjc zxg)+ZSD9Ak9;^*^*Lm(UT;;xU|AnC@Uz5Kn*zRlhw+GjXtKDmVjjIAz1#fb1Hn#Y- z_-_f^t3K^|Nq^DU8~9xPH1H31kN&SfE>Y+x&mq4n5$C-ly*I)%5$)~1JtDr|9Je7} zucm|Msv<5L@cUCfH2^za4OG*VOAWYOs-LE>xI}sZl+Y8TI`MLXB)UoCE-&E13Urex zmaEZc`N$Q%mwm*4*Nuno_TmyWDz4H)3|p3|4baAG+ypJB5e-`GtfmtZY3z!d>p&R* z^kt=r;_tpFYxes;^saH#e)O+Ys*I>PoU##ALME$^MIf#wsrp#F%l<}9@ zc@#rfK3d4mM+0(&spPuAIaLi8hK;n~$}WE8{Y*(w<}Y2@Mdz0eSU0UUJMS62yfnVh zTfmq1+_!4Oy47Uisb4=oylFDE&!5eHfNwwtkc!6bQaL33NbJTFtTg5tzy?rp15ybG zJ4hTV4I7Vtu0TuCbuhOJ=wa6*_p&ujOlP7&Vp{6Q^qymwd8cdYYG^K1+2FxLb|08< z>7e?-+<^njZ%vPlwq6GJotFEA4+;A~PIV8KRMm=LVS+ELNC*|~q+5%C>gh9CXwZ~@((IbhJY7wfdmgB!U*xNAY^1R} zn4^!qs=w;1*Y{@z(I3@_$_01GJ<7e&&5trJHCD&DNufphg20^6D)-vJM)&5xE%7JZ zs$jDD)J0!J$V(XR3o6j#JLx+zUc~g>ac*1(4-@1gd&%P{O6FS%^iqIsx_yp<5y0u@V8xP$P zeFN{sLut~Z<$`u5qVMY1TGfs3+I}mWCNjrPz&znhX(ycQe$ZWdSS}DjG?BMS(a zx}CAD#q>ItKx9fo&V(E2c-QnvqeGx3PwOgwx_jYu&p&!i%_RY!Yi0Mw1?^h`UB#dO zWX*3E&Yg4p*6h)@f1blPMDE+vdHuRa0uPck*UY~D#v9FDznZsW&J_>zFZ}84`?CM| zghnPdW!w2;n3H^{5O1?OG`*jGu0BTR8_iCW6qyxTvS3hfP{HtmW#(2>9u^)Jzc_q< zalA>sOq&^=8DAhT)Y|pM;f3*i<{N?cBk#xFC_EB4Qh3BzJxn?*582gE+6Ek}zk7!tvKqZ4*$oGnEMM(;UO zYZD7l3!PZ=(I{w(miwJ(HjF-@R#KV4^@&GqdVJVj^KU-1VAY3L&$y$%@#N}%H9vZ) zW99a2yYSP^6DMxT-TPSf)UB5c>p8_eeqjG=Z@u>0chFh7P@AQdlX9IXst$avP;E$k z2p%2RbfJg1%RMwIl+QV|*L-{II28lFu)L~DUBX2k;`y;JuP@OD`j*b*Pl9${mWFQ) zO<>VU&jz6Sl5@+h+VIkhu?Mq%6Y*jE=a=?u+dShBr+VJ|GW%6lhF;zPR^4IxUQT?& z9u%WtCQ!wR=?~GD#vHezzCdlpk4t5LNW=aROoFNbRxzpxISP2lA+AtwhEgaM2~!O` z#_PP%*HTNl)}BaNnxgMhkNzyFiW}!+Zn$w+PN^^7sla#aWN`Q%c$BN zJ02s;Vh_hUW4Rbl4L`S|eAp@pm;t>?n}U&IDuayo6@VaHTaW1k?4I zx~(vETNxKUOWn>3pnp$5vK2Tgks2Fp#f_=jF<$q$y>6NiPZu6>qRwk^JPEwqGf6JR*EoxlUIW&%r&li>+;9O>#i=Bv1H=J+t2BGpzFfLRplkd2h)Sr5fjwle{5VrPhtmW>TO?XxYVSc?^pM1fyas+7#>)9EBXJrA}eq({< z{IT_ILx$Sx+-mFW?SO&yw!~fsXQhCuyh4%ikno|vkB8#8z!fonpIc7gL8FEl+g7sa zm6);}tf{Mg2;qHTk(2M#xWCEg)$~m*W9ffL&MV!{>r`?%ClNcJ1c=7*XIoFU_sY%# zWQ&bf{JXD;+C(>Wv1M|A;(Ze|TWM>t1(QFvK=QTmnqn}l?c)}&2;@hsHlc&_qY<-f)ElJDc# z$MNGa?Pb>seiGLcdVyZ3i$BX92P_^&GHex)JF!AlmqqcniC7?!h{=fMY|8Y;@zuF8TzBc6lczn(EAMsw0xv8)8_#gSs;O1`He3KJbfV1JIL>hgzDktC3tm zmXhlTPxg|3QdETR$ZHx*&uGw3oJqw$##D>2ty|ESKY8-Y-C-V~-c#mCr8CNkCR-0q z1&h-|2Im(Bp3yUC>q_x_y6tb`>wwJ0ldfm_3z&}dEry9JUX-Rx#!Mr zn7!}X{U6U=_`L@<`wzbR`E%2sed+O=TL)6w^MhJ{u^wiYFs`r)0}2L1Dmox)MVCc? zpgrJz#x2L(aBu*IZ`Eqsh2$;543}E$XS3T(`X+PizJj6 zlP4UZaNJ@pW1{!*iG~Fksj`Sg6$y$gU?P;Vcl=am-anwff551zq4p-FuJ{Xe0!Af9 zJq^KGZlhia`4Y71KJe@&__p{nQz=BD1rNAVfs7yl%?=x0ZNQbm!> z7kjcx_;k;W^OjCoO>VZc$1QF)c?&o@f;L(gyRb`*~l+!DA4!Yve(|}H%;UQ)x9|kA41X<(Mwe}|vw6mz z=s7}VTXREwHH~9A!i}!`&o7{V0GlZc3Z*Cw9ulT+qLb)Sva-QL(pLY6F7)F&xxl=e z`d@d|xl7j!8-MZ8)g6O2@Xy^ov|`uD*>~4g-CpUbyLsyPn{T^#>K*-~)E3$U54jOI ziM~Z~sAcj35+#L*4LsL?MP4Nh)if`Wdh#BT^S1i(V^n3=k2yiMfdg27@;&_a95`@* zYdUb?^iu~8z_mB#j`H-KTskVizp<{yg61t1>V#2(&{))2M2d<^5;ck837YjM4)fC# zz)M1x#G2)1_jGS_=_J<9Au7aU7R z2Qx;r9lo2hB&kF1q_Vmx5u3e|xcDI47hB$rF4qH=%-=u3K|m9JkB{t9uqlU;IU&{u zc8Q_bxcuX`M`z%5HdVP%Chk>yrax8Y-N`2pHsgLe!;MS}jN?VWRbq|yRS)>zb^oNi zk@(KRn8PkL@PzW@4&Qf#pE~Ww6dtoqX{N0QA^41|TH1Fj9jWN)%t$}@fs%DONERX% zw&a>u1}`5oCn^DL^)1S2oKnHb#?f^4>RfzjPXw^7>__I`>c#l;@Oi6j;U&?j?`86? z6`RKGA6`k6SJt>T5AA!f{z`A}p=k?F4eDk((eO6tq5J**2d14dw%3F<*(2+IXV5$` zy|k4_IB7{-6AX|W%jv4TTxOHCZ)2ri+WNFp&$K|l_?0ycP_AB@DZ;q+nL@PVCgsWa zG}8(`mNhnC`4H;+bbGg@=;@Z?rO^7MLjmL&nX8Y!%6lZcrGvYwM_rCsmuH}_7_w|* zLwQc12k4B$47Atmyg7GVqwxOL2|Xav+^eNmG4sXd5ADh-mdu5Z<9*C2I_EhLWI>&M zG#JGt50hNLB7lzkP^bGV-oj0>(X{yW;Zu9U)ji?R?BM*3&^-x@{8shA@k;;dCm}Bm zd8PbRYWw?*S$^&cyjk$$#@!csQyTQ6_qgJ?%@q7)q1L1u-%-;&)LC-AM35&~&2F?7 zo7_HNUV6r&pU@@Bfbh&M4F<2sRVc}>lNMZtV@5=QvW?igTqCg&XpKTAqKPRaqri0s z-{!-l-~V=Z1!2O_`L=Sawn&ems4VHmV}<2sXF5pVGK4^ZLr%p*RthC7OO<w&zvF8ST;m|h15el=y>MJsFmkjO3HrNPTUKhv%@SzwvxqzSxNGctj_D- zRj9}S&f@iR?S-6Ug}I>4Z0odJ7U^}#8JISXz4#vovQ%3pJ7i&Gctge3yh!#hI1S_4 z*K-i^Z4X6{PD{>-Bx<8oj7P@|lz#5%_Z%_Og8O>WoxcZnLV(aJpopjGFn1^S_s3%# zOB&%t>grCnz%6LUy0-52$(^oN?nRZ2`sGi+^UoXS0k7zER?H0f1=8tWd6ryGZ7PMz2>CGZ3Z9}Ct!4JHaI7F+l!r@X8U#Ce_iQih}U^( z@-j|dM}<32b6Q|rAkSo4v#D#Vv*G1{*#n}k?Nb@cx@_k?9`)axj13=E5n*Iib1y^8 z0S4t)*~V7irSC55$Ybv#gl<+T&i)b#@A7<6R9g+Vr7pRfEMrj>Q|+$oACggfdF!rL z<-GAtTjtH>rK`W7qmOB0=3%AFaJzWOURAx|RDZKm7zl?t8OP}#X7W!j4{&R1zRjq3 zrYebc=HcN9`4lvDR*2kKi|dGA+Bcm@0QdtkJJ)RjMwV`kCh%E&O7;dNW9UZ?raPxq zbmTpDKW$rPrEi$o79Wo***Sb46Y@$4BqTzu5?omhY8egKN;9MQbTuA4P=1!1$yQSd z+fnK7qGno`*t?A=TFuJ7$JWtym^hSve$kv>6WFkuNK4!~Jj?5_{qkYbq-p*8ccA`x zxs;df)$bh)$PNH9X!OGb2<)Asm^*(~c~0m`HzN*Ib{A+PNP1am7EC#Kq>zTQMwTsu zBW?3-s7wZ@I82mbWz5*6)y%6$FMdri%dRAw+TI#Ax!w~6-I+8RSxH%mzT5Pwu=uU> zLjeKVbS41}2rcB{ECksrYns&I4Un%&Z9ODStOAi91Eg%-+?e?3PZv@?q-bx!Keuaw3&D zSR(ERL+M)JZ>&8FYqsLn^ym{}fqtTGf0qK$@0mAC?Bx!c1>+Y$lxy_v1C$%RvwWW$ z;(Svw%JJ!;{h4MM*Oc%#z@YJ3D6%&cHX}*x^C2g2e(BO&8>pGaeT>${(iqfmX^F&# z#iE-v)HEzcOQ~S5V~F}E`-qC#iW=o)?4Ba_KvXH_kpe+U5YgY+`&whqjx;zL@cX*a zD^u8Vmut0_OZcXk&hO4XWUS$sUYw$$6g}2|{5Folo`(#z$<^y;ZSLY4bW|Ct>yhYk1 zXb-2>-PuZsahr!?d=aJSgSw}-7UHy{_LoePV;CBlC9`6_d^6CT747mlUktbv&9rkn zXYbAZ&43JG_Y)n`s8bLuh+?dtP6biDYc=j=^X8X~ALH$gr;OKm~mI zCW5`G7x3kK)u#M)F8pI(v7(b)&k|Lb!zU{IZWS6pc}f_Pn$fF9v<0|_QW%aLEtoHo zSL?}$P#EUXx{KztKf9A1qYIj$^1*e@)k*M=yEWY*&ylZv0|x;5*ix)(F!=5**)KzW+a_p|KkeDcW?nff<3bfmtJ`j#>5D2N7q z9C=a`apZOI8xB&&gNfvY4^UnVJ6fz&QM)|CeK$o94_mWFPZxT((xgRVW+7I`Mx*Y4 zVy%<(IMqk41yA3LoaXaT-uCx!-{XO)ipV=J?Zz$VHiuW;G%iHae;xHn^nV|Um2gRi zRb-He=Z*icSY1{6=oXv27fK$K-<(V1XEXqX$4sBMz@#Si-xhe}n*+{C4!930{Nm3-k5@FK_$+h)cCzr!TID+&D9MG*4h{E1qk~_qjG>Jdzj8 zy_!ILe#3C`HeOixk@8^)L0hL{)Ly>+L_7v=FIANx`C)Cy2kcrlp~Nq&{pUM&-6 zn_$vr?J%@pf2n@h=+PWV~ar6 zjIi5m(MN^}gMR&z%-TD`)h+`{C|uRHqQgpr2CK4pyOvsn z(Rczwwbue;OO3w0K<-(-=u!T)UmsNq2ZkA{^_n4>b_w~SEg`-uScp0DV zoG3Fj)l|W`8hxwm^S>-Ps=1l~P1e_bjk4-XdO!K_UT1yfW7B#C*2Q)xcyso1iSnbd zgp&1~BkU++Z}>+}uQ40mg+fnZR)a-R&O>lstJH}3eXwG7+}@2Linot@rZVZCO_sl&GBmn8qCW;t zdCUhIzy0(OosB>hbI>$#>TuP^D^hRMYBklYc;>;mpdwn$uH$Xt!h`ou`Mu99&ZkO# z#rT|m3t2kOdHk_X(tJfAAJ_<3+BoUJE3*u)KX#{V}>LvBda7D*7Ud&uk7evWxk#MEi*;5Nt zM6Oux(wBkMv&6x1`U(k(BRk(7cQWMJ6NM1t|U z3{Q*Mr-b_SMaUJbhy)$ufJE6mK}cgPnDhH41o+|Qea;-yJX+|oxZK);5Dg?ekUxvC zz-;SqE@$dzYDX&`w7lU0bDs(AKT*>5R5X<53sXdsC32j=n*#mCxZYgBX2^QHm_2_X zyPNXf-I-zZHQCBNo-?kq;pcUUa3`hy)q1;6G2h@xB=?Uvjuv->MaI_yCOBU9ppr;x zKmhgQ>)AsWprhkYXK(wL?%^Nb+yo7Vv({Z|{S=gBdL%$(MJu?N0=T8=+xp01dWT<~zfLxZTrDO}H1&;DfH!S*A!{fY5wHjys{VF>*CYfJkPT z14dMu__TwEeznzcQ{4*WXuR2e)Dv-QZqUbelPNUpF4O6Wp1DUMaE8`5-o%NK#8HQs zlQJ>|5dHz|V5JqlZV`XYuK*a=0xowpr=5*%T)Ea)`3-pvXF`coy}I>)d$GdaFn+V6^6^xicgiJz&5ZZEyhk+6T%SfM=nBd#~+#NSMh2 z!f&p{QaF^yAgtGDjN&b`LGR|U@?%2d@m}V5ozNb{;r{jtl zK0;TunTFN&=J7LJl#V>W@=PsqN>tG>HUDnx82(yO-CSXRv*)E*vF0wt+)^k1YDIkB zfge60#Rf_&G}a(i*qF@d5-(3~W4374a!z$abfY&a@=Bh3$SP3d7M(?|lj5&lW^%#b z)WdAGr0U{D(4?AbU!UCO?0@-Vjb66pNVyzVX3G_b!QZSEEO#BClTI** z>2=kXEdzDi{~S%`6L)m>G;{ZRv-mmRFXl`3=ULZTleHS}T5L#zp6NYFf!Y+CHwwNZ zDi00}FZf63h*tw$lJ(Bxt}vwwSOhVrkDd7EYGfwcJ;#f1_ujZOj<6VK<#^tog|AI` zoK~~oHx4A(H@R?M<36P%v~o(I_dKyXSt(hYT>NKm%p7C6Fwgk_aj?JaOTXU!*f+fXaM!UM zdBmr^p8}6Mz*~`eqYVB71NR^L_pVs>dTC5#E6g>&<#Kc)D>J_5(5sm4MC-X8@s%_( zLd!Nqq&JgSP-MvQ#!WP6p!Nz?NXo;AqA4!2wVn{AkZ@1e=bmc6?oXew4lJjRK|Gb# zS4W7aT4ls~pd?F%Bg;XB?fW`y=?|L5;xNlb@OGwJEdh>Fu$!f->^1V6s%Wk}TE zLV%U1zgcxML*I2a*R>F|zuEOAy~=5x)4P`OEq zS&V*1{47HmPuafXnoz@|)J8U1gc0W@T9*WugnhaRZZ^aIdgeslEGnuf`XXG*xQ)LNu-#V_>3zk zFchpwB_V+cMzNA0;KIo~?0^Yo+_e%)CwD9smA+eanfCb?rR$nm2u((UhSwemT}H7& zywo*=Z@b%>iOrf!1y*M)XSZlh9NU+oUB_^ixE+5)j6h@ZB!N@d`6zft+IFPTL{RNs z2(m12_&~e8Jx^Z3rN7XOui;$0I(Nul2szq}J9J6w9!#@hN?2bf0*>i}?W54(gXjJB zu~ii+qcM$>Ug8}yToX-Ou8XB>t{qDyDY%~Q?Vf0F=fd~D`151hmi1%q*~JyT8nb*| z4!^dW-;^moaxoP=14FySG7jWaye$^XrcTeur=0A0Bq_hHI3GUO$L1Wh*UP(1F_YG( zJSKTXGQx+*;D4_}4lor|f2V!L4Z26acsanM`KO;}O;k6=kQ(61W4urk<3G>Mv02`G z+uMg5xx;_ni8A^fB+hhUdic{XE4;g%DO!@dA6LnB-r0KnT(OI`w>vfD%7y6<<)2*R z{kS+5)eA#vqUKTA2#!+y;@!`(66m?|+K8Fc@}3Xdh3Prv1+Ur1rr2A!GB63CI*p%fnjhAM*=Iw zA0seK5Hpzk_y+Qi1fpr<<>upQW#a{gAu!A-A^#U#o?`I`CiFjkGHyPu-e5RH&cV~m z8w?Zv%SgrI&-|AumBfGihcHGp6^Wrvz0dZ?K@*~8UZG6En zVa!>|J&Y;#8VG@L!NRCasQwPdBx0B_16Xjs92{M90eqAb`5EGCT< zM!?}PS(LP}pez(62NRT*5|S21NTc9rn4B<7g6jXLfK8|%vTjx~_BK{dUOp~hh_Dn~ z3N0ul1D6&Ol#)hEp=4x*5OBDV2m&UJM!?ZBf4bx4ZQ<$t*UsP~2qc_}lM}5jNA+Ji C Date: Sat, 26 Apr 2014 15:13:50 +0200 Subject: [PATCH 063/320] Use INAV as default to not break existing setups --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index c75281fcdd..268eb9bba0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,8 +5,8 @@ # attitude_estimator_ekf start -ekf_att_pos_estimator start -#position_estimator_inav start +#ekf_att_pos_estimator start +position_estimator_inav start mc_att_control start mc_pos_control start From ad1be765bf36a6068fda25e3c62c92e275aebc6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:14:23 +0200 Subject: [PATCH 064/320] Fix warnings, use more efficient atan2f where it can be safely used --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5de22fdaec..6eceb21f8e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2250,21 +2250,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // 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", summedDelAng.x, summedDelAng.y, summedDelAng.z); + 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", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + 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", summedDelVel.x, summedDelVel.y, summedDelVel.z); + 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 @@ -2381,8 +2381,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f float magX, magY; float initialHdg, cosHeading, sinHeading; - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); cosRoll = cosf(initialRoll); sinRoll = sinf(initialRoll); From 9358eb2428275f78ad5b1e06b42840927092ac89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:22:03 +0200 Subject: [PATCH 065/320] Fixed string formatting error --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5315c5e387..e260aae45c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -655,7 +655,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { From 0d50b3ea866a9ca0b9271b8c861f1d9e2a61a24a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:06:25 +0200 Subject: [PATCH 066/320] Fix struct inits --- .../fw_att_pos_estimator_main.cpp | 67 +++++++++++++++---- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad8c1b4c00..1c943137a1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -103,8 +103,6 @@ uint32_t millis() return IMUmsec; } -static void print_status(); - class FixedwingEstimator { public: @@ -305,6 +303,25 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), _estimator_status_pub(-1), + _att({}), + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + #ifdef SENSOR_COMBINED_SUB + _sensor_combined({}), + #endif + _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -356,6 +373,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); close(fd); + + if (res) { + warnx("G SCALE FAIL"); + } } fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -363,6 +384,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); close(fd); + + if (res) { + warnx("A SCALE FAIL"); + } } fd = open(MAG_DEVICE_PATH, O_RDONLY); @@ -370,6 +395,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); close(fd); + + if (res) { + warnx("M SCALE FAIL"); + } } } @@ -538,12 +567,19 @@ FixedwingEstimator::task_main() fds[1].events = POLLIN; #endif - hrt_abstime start_time = hrt_absolute_time(); - bool newDataGps = false; bool newAdsData = false; bool newDataMag = false; + // Reset relevant structs + _gps = {}; + + #ifndef SENSOR_COMBINED_SUB + _gyro = {}; + #else + _sensor_combined = {}; + #endif + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -624,9 +660,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _gyro.z; } - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; @@ -672,9 +710,11 @@ FixedwingEstimator::task_main() _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]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + if (accel_updated) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; @@ -885,7 +925,7 @@ FixedwingEstimator::task_main() rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; // Copy all states or at least all that we can fit - int i = 0; + unsigned i = 0; unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; @@ -1173,6 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { @@ -1185,6 +1227,7 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); + _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1259,7 +1302,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); From 2d978beefbc010d962034a0f0d588cbf46a063f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:16:44 +0200 Subject: [PATCH 067/320] Compile fixes --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 6eceb21f8e..ffebcd4776 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2307,7 +2307,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, states[i]); + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; } // state matrix diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1c943137a1..fdb5dd5883 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1213,8 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; } if (_local_pos.v_xy_valid) { @@ -1227,7 +1227,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; From 18a932fe948b06b87d65f5d13102274e5683fab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:35:22 +0200 Subject: [PATCH 068/320] Better fake / simulation values --- src/drivers/gps/gps.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..01be80dcec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -276,14 +276,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: [PATCH 069/320] Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..148a0aafa2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #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 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 925430796ed56e97bf2310423361a8e6e29e350b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:37:26 +0200 Subject: [PATCH 070/320] Reworked how we deal with altitudes --- .../fw_att_pos_estimator_main.cpp | 79 ++++++++++++------- .../fw_att_pos_estimator_params.c | 11 +++ 2 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fdb5dd5883..a02b2c34fb 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -191,6 +191,7 @@ private: perf_counter_t _perf_airspeed; ///yawVarScale = 1.0f; @@ -568,18 +574,10 @@ FixedwingEstimator::task_main() #endif bool newDataGps = false; + bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; - // Reset relevant structs - _gps = {}; - - #ifndef SENSOR_COMBINED_SUB - _gyro = {}; - #else - _sensor_combined = {}; - #endif - while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -611,6 +609,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -618,6 +617,12 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /* Reset baro reference if switching to HIL */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + _baro_init = false; + _gps_initialized = false; + } + /** * PART ONE: COLLECT ALL DATA **/ @@ -813,10 +818,17 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _ekf->baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude; - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; } #ifndef SENSOR_COMBINED_SUB @@ -904,9 +916,9 @@ FixedwingEstimator::task_main() } } - // XXX trap for testing + // warn on fatal resets if (check == 1) { - errx(1, "NUMERIC ERROR IN FILTER"); + warnx("NUMERIC ERROR IN FILTER"); } // If non-zero, we got a filter reset @@ -960,7 +972,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -968,26 +980,37 @@ FixedwingEstimator::task_main() // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; - float alt = _gps.alt / 1e3f; + float gps_alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = gps_alt - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.alt; - _local_pos.ref_alt = alt; + _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; - // Store - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref = _baro.altitude; - _ekf->baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = _baro_ref - _local_pos.ref_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, + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); _gps_initialized = true; @@ -1089,9 +1112,9 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newAdsData && _ekf->statesInitialised) { + if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); 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 index cfcd99858e..d2c6e1f15e 100644 --- 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 @@ -258,3 +258,14 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); From 6ab878c0218f26e5fa71053b75d7075b594b937e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:38:37 +0200 Subject: [PATCH 071/320] Emit the local position against the GPS reference - this means it can jump. --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a02b2c34fb..a82e8d7044 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1203,7 +1203,7 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9]; + _local_pos.z = _ekf->states[9] + _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; From 6a7b104c2baad7527d35736979ddd1352bf4119d Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 13:12:28 -0700 Subject: [PATCH 072/320] compiles --- src/modules/commander/commander.cpp | 5 +-- src/modules/sensors/sensors.cpp | 34 +------------------ .../uORB/topics/manual_control_setpoint.h | 4 --- src/modules/uORB/topics/rc_channels.h | 4 --- 4 files changed, 2 insertions(+), 45 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5f05c9916..50380107d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1228,11 +1228,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { -<<<<<<< HEAD + /* LOITER switch */ -======= - /* MISSION switch */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a750db12e..6449c5283b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -255,11 +255,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; -<<<<<<< HEAD int rc_map_easy_sw; -======= - int rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -313,11 +309,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; -<<<<<<< HEAD param_t rc_map_easy_sw; -======= - param_t rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -534,11 +526,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ -<<<<<<< HEAD _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); -======= - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _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"); @@ -679,7 +667,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { @@ -690,21 +678,12 @@ Sensors::parameters_update() warnx("%s", paramerr); } -<<<<<<< HEAD if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { - warnx(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_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx("%s", paramerr); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -745,11 +724,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; -<<<<<<< HEAD _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; -======= - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1502,17 +1477,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ -<<<<<<< HEAD manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); -======= - 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); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c61987df62..b6257e22a6 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,11 +78,7 @@ struct manual_control_setpoint_s { 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 */ -<<<<<<< HEAD switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ -======= - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index df651e78d5..d99203ff65 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,11 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, -<<<<<<< HEAD EASY = 6, -======= - ASSISTED = 6, ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: [PATCH 073/320] renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++---------- .../state_machine_helper_test.cpp | 24 +++++----- src/modules/commander/px4_custom_mode.h | 4 +- .../commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1_main.cpp | 42 ++++++++--------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- .../mc_pos_control/mc_pos_control_params.c | 10 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 +++++------ .../uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297fa..aa9c645028 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d0..a99456370c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ 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.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d1..b440e561b5 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d34607..962d2804cc 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c74..21e36a87df 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae072752..fafab9bfec 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df7854..d5a68e21f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce16455..108ef8ad46 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; 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 104df4e59b..015d8ad160 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6ea..2cb4fc900f 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bddd..59bd99db78 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 11:41:40 +0200 Subject: [PATCH 074/320] Reset init flags as well --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..6eccc4c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,6 +2407,19 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // Clear the init flag statesInitialised = false; + // Clear other flags, waiting for new data + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + // onGround(true), + // staticMode(true), + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements From a4724acf82e48d914bfe6397340454505e14decf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 17:26:42 +0200 Subject: [PATCH 075/320] autostart 10016_3dr_iris: yaw PID parameters updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index f11aa704eb..084dff140f 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -18,9 +18,9 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAW_P 2.5 + param set MC_YAWRATE_P 0.25 + param set MC_YAWRATE_I 0.25 param set MC_YAWRATE_D 0.0 param set BAT_V_SCALING 0.00989 From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: [PATCH 076/320] Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/estimator.cpp | 33 +++++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 7 ++-- .../fw_att_pos_estimator_main.cpp | 13 +++++--- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 8ac2d61541..1ca34ec307 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() : gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f) + VtasMeas(0.0f), + magDeclination(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED); + InitializeDynamic(velNED, magDeclination); return 1; } @@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound() return 0; } -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f magY = my * cosRoll - mz * sinRoll; initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg -= declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; } -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { // Clear the init flag @@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); + magDeclination = declination; + // 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); + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states @@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) 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.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; magstate.magXbias = magBias.x; magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; @@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { //store initial lat,long and height latRef = referenceLat; @@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED); + InitializeDynamic(initvelNED, declination); } void AttPosEKF::ZeroVariables() diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e118ae5734..378107b698 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -208,6 +208,7 @@ public: float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination 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) @@ -309,7 +310,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); float ConstrainFloat(float val, float min, float max); @@ -334,7 +335,7 @@ void GetLastErrorState(struct ekf_status_report *last_error); bool StatesNaN(struct ekf_status_report *err_report); void FillErrorReport(struct ekf_status_report *err); -void InitializeDynamic(float (&initvelNED)[3]); +void InitializeDynamic(float (&initvelNED)[3], float declination); protected: @@ -342,7 +343,7 @@ bool FilterHealthy(); void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d7044..23f9e80bd8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: [PATCH 077/320] ekf_att_pos_estimator: local position reference fixed --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd8..fb019a354e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; From 31089a290ba089b2b5cbcc76ed677e3f401ffa36 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: [PATCH 078/320] Replaces poshold/althold with posctrl/altctrl --- .../include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++---------- .../state_machine_helper_test.cpp | 24 +++++----- src/modules/commander/px4_custom_mode.h | 4 +- .../commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1_main.cpp | 42 ++++++++--------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- .../mc_pos_control/mc_pos_control_params.c | 10 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 +++++------ .../uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc35..bd3fc66e70 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c645028..974e20ca26 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370c..be3e6d269a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b5..18586053b1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804cc..e6274fb89d 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87df..3b6d951351 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfec..dc82ee4752 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5a68e21f5..024dd98ec3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad46..38a5433ff7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; 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 015d8ad160..dacdd46f0a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900f..6d46db9bd1 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db78..02890b452c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 22:11:02 -0700 Subject: [PATCH 079/320] Updated flight modes diagrams & comments. --- Documentation/rc_mode_switch.odg | Bin 20160 -> 20920 bytes Documentation/rc_mode_switch.pdf | Bin 39286 -> 27305 bytes .../include/mavlink/v1.0/autoquad/autoquad.h | 4 ++-- src/modules/fixedwing_backside/fixedwing.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 5c4b617e609a916e1b88893f3e72c31fbc152f5e..3ed379d084b834eb7fdf30ad009197deb484600f 100644 GIT binary patch delta 16014 zcmZX*1yCO`*YNwt-Q9}2ySux)dvS-NixgUl^G8c@cQ5V~DO%j!-Cgdq&-;D%eeTU~ zcFyD^C)v&HB%6~x-I);0`4A{-icrwlAP_tVlt-7DfT9BRSD0}A6G>YC#IeACl;mJS zB}8xx1pMDDm=1#XpPYms2b(D&BmM7YOvu3(N{Ihk1)hZ9{=d~Pl%PMjnmb!Nvi?`^ zQcOpeiMaS65H3DA1X3KpTa}j**ZTD1G}H9!@?6#+5b-=$jC4;O?j|RTD~(+LnS|w+ zJ^WZ<%PimOaeA-Vls6Rpg6nh@a*rWCEC-aNnIQIwvVTtm3bH&s_IEe=0S0w4OSFb0-P zPVX^h9!CtFRUCpW7ZxV?Ga)tz(_RfESMvW+3{ql79w5!ZeYLTGgSz$<3jd+^!BqQG z+V5sKL^C?SrH)}X(d6nYC1dNS?02`ccn}W{PsFJ2>ywwR0|(&&+ZOZFV^f=MbXXJs zSwr}mJ++8&cNK;|Brkh>kFfK_^=02x8q|t`=5nJ`;eWXMi4N-cQ0M9~jmhBzoX?2R zOgy4Bl=4Z)q;xcy2xhq4F9PmbXCn!j7`L0RXg$UxR+r&;E7e z5njAgiGjiBpjLW@D|KIF7%#*i^hO16Xn3guD{vctvID)7T*W+FR-;2^GhwZ3Px!bm zzAF)_$qQi%Emq-?b6CEJx7Kz2`=xjLwL`Zq84Y+ts&LA*jePP>d^j!C{MnYlaUlV9 zYix0l!1{Sb0i`UefiZ=Fh}LxIs!+})O}g7K}u{U+Q)&w5=5{FD7$X8gZ?FyV>u=p49X$*1U#Ly}Do?_@!h{ARv- zT4k%uslmRrBeCuE`7X8sjlqzW8Ejaigz8X$MR%(tXbX{-p3;U+LB>bH0 zFAE{lAw~z;#Un2w5@uH;8U}p=_<}#hb_w)^K$C`qL)b#+tV4HanyZnxpTYq`L^7eD zEqy?%T(TZOjU=z1!z26_6Dn^=g^j>>d$wl_LPxRS!z^Hv0&tRl3C$k=>{J3@W<9V9GYL%@$fr5yC{9zUZ~ zN?my90ZuQf27C~gs$ z`Y4PiX{9F@F-o;mhU)0CwrnDb;)NtC`6GKjhPSXcIdG|g2F2cIZPy-B>))TxeclAH zvH_jN?1|HrtvIb<+rw~x+5+LyLD1n#lliT&nmDHIfe9}z?)lV>Q*+1j5yLQ3q(!;X z&*YwOt;#z4+w*IhtTH$nNB4@3#{s@BeM}ek&&M`dKI_SwU2@iMmzlS`E~<4<%sUOR z^;ejNlU<4)Ju|H4q8^A`L`!#XxSe#a&hgoWvKR4T z_$|v!jhWK&6Y^H*i(&#y86)@gQt!U(6^vV6zWl5bQ{ta<(33&Let74TwN9^Z$*~hLL*ukM`vH zm=0tLPYB>=OX{>BfJ1lGVMqC`&H=gL)>a#@Br2(9r$|lw{ zY<*)~NLFLH18Z67l10+#Fyl3qTyc%vz0#Io76c7KcE9?>xsfi#K;=LRXgQc;Bls#^ z3S;Z34a714jaJ#824taqe0-dx>fe-S%GAa@a2C0i6I-pfu`Xk7`ye^v2(J z{U`pxv|5vLS4N11SUxGrL_kPI*Vg%wY>gLI9q9{Zd6r16NLOw4)ZTg*luO{<3wO8- z#FXUg0Y*q&^475A*EtW-_Mq#XO{mKs{1|UNHYva0{p2uhc~wV&VGHQY$33)n&sTPO z4?s|wcAUsOoJWMJ956I}$E*stI#zc3$gZ?@({@M?%jM@npaAmL^}F4KySRYp9uf6O zr|lXSVGL?j?)#ypX|qiF9suw9`zyTX)eKX>^I2rZf@Of#Z#M$mEQ?mQEFsLAt5 ze(I5nk-)9i?l-@51|u9hwa83I$GZX^0C<8Pbvdz$A%E!ciAYdh*KRzRrSy`7_%s57 zK>mLCOhb1s=67A_cXYEHx0#u#+E6&{0@Z<|$H7erxoWii_AJ=!TlCh0R*iKXNcehv zO>3w+`^EH_J#@UgD?wt9SLBIu)sDyDJlmW6+h+EQYJRbwlm`3pqqezXHSqZK z^rVGsM0d!AU6&I~l{@k#ML-t(#u;eG8PRAp)JxsBlhFel^oEULvHmCuSs(4saM;;i z8B_H1^^S-L&A^#3b{c?xx{H+N<)jZx#=W9uw2G-3uj?E0)@|H3`d!-6Nt2tjrc8rn zGH+?0R>C@2!WlSG{rVK()c*}d8;}d8L8ncxR_Yb+vbMsHT}Ai+EIp?wjULeQTWaP- zus;6Lr8$V^MiyXahdxk0=P>B_ylEn-kK~6Q9>MB28v$Bs zO`#RhTHwQ2BrEf8t7ZedPa=GidhRR?bbPiRoqkW79iV_?*y-#051mfie3aZNKcW%s ze_`&|XW?0k5^y+Byew_kbIxwb2Oz(xn?MRm~HL|aQ8Gkdq1$@*@7{=T~K zEXe3bSJCP>XXc?_kB<*;LE+(#Np>3y+{&1TN3bhFNqRB%o~HF&4(e1B{uc)&PnUx- zro;VzN2?vsRByK3+95sjYCuaVPA=$iY1JAb{mVP+&% z`W?gx|e9;xZz_5Ge0?USg79fM*AK|WG3%~k2yRhyWry9 zL+ts0mM9jdy2D*5E05HXW-*bF4U9#0bWJN)KP8dTJTragrf65!yY8rAvvv8S=&BGt28~d)P)FdJcm}X%l!5=#K=D|Q@fi|d z4DCJ!Z+_ksTD`MzKKjNlfmsg;0Cm_E16gDJ;1K<9#`HlKn#aKo z?u%xET_+5z5r=3V5@XN#s&+6*st!{vEF6;qaug>*fuaM&A?$d5 zrH6#s;1*9uWiQMD3E6%tmV^dj2M<1Y3rNX{L6B(z2v3mYU!ih9j&3f9_rf2!C~W#S}Ilt9?QjC6LjDcaUm zS*qt(keLiD30$}W#bg9Xiv}iOEN)!wGNzf2Kj>llKB*952MfZhtyW4(ltW%HiOZ)Z zGC6Ajom#VaT)`Sob!Jk@3=+vMRKitW@Zu27{og=s-v*FEYgup-59JEvETyOw9m!L} zpv)_~^AvtMEZ2SDC-H{nH$buF ze0)+7kTudidVFV*-oM8nsAu5dR8bLgNGTn<3!lg~Zm=-1K^)3_Zvp5qY@mAiu=!iD zP)Ph}=ciR>|8lC6rPtV-x$t_Fvj^&; zV`MCpoGz4T4FHoNo`W5eaORJPa)fG7vFzW$7OH9>N%e_&F%>bBL+*O@9EkB-DGvc% zZmcLPIf-=T$fUZK?|a6l82)OYKDG65d+w|7ctZ^7xdFS3#f6_M1LBpi>C$=73~9q! z+)QSq-Cj(^Y2e*Z5q;g1v8aGtHMZ;DmDVXCj9i4_dw?G!(U#zUZLubx(f2@)dl_3^ zD}ZT$aQ3|1I+a`qUU3lq%sXqO>m7T|>_N3@ms!^^3cD*um$Zs^OUUb`N)5seu5s!k zk~e>4#!4%{+@d?dg}4!~2_o4iXP!WoYssnoJ&@&Lh{uj_zshRE>-F?DiJ1}bcivWN zubn}&c-_Tj!X?j|)|$#U3t5#fFFPbI+%K!YF*+lg-os6{j|jfkzn>t2P-@3Jpz3RK zBDTehDSU(HuM~F_W&Q@o=x266B%b~#K1;p#oYaZTC7eCM^SceViH@2z#mpa5Tj&EA zEBF@fSyRBFX*k=0%|WlLDG1oFMvt4&Wq7_^88`5v$85=%cBb~O?&o-B8WGap<2TFbYtZYFe}#o%+2M=LX(P)Na)N>q*|@MUz6K8vxs zg320qK{$1p<2C011g=TkSjhKS1<)&;)w!GOxnWrlbG9*O&2PLj+JO#!&y~!v-)X5P zyFdwu!Q-4;t2XQ1NRdSnaNZ+?11IE($2?X^l`?v#!Y^bHSWWQD7p+%48+3_YuSQI zs@HOYHo6mPG6ISZwm>`N>j4s1`>*?+!;C)mi0;at7Q$UbNzYGL=io<~5WS;hIjIP( zPNUZ}s+6QzCUi~kc8dcedf}sjyVdpgB9ns3&?)2Z8pc&u<&jGzO!_;dr)I-z}`esgYBGr+l}h)fMSW_@jY!f;&ud;-_iUuHN@ zx*o$+-0*mRen4b8tkc=y`HiJ?5l?RGW#ElwC+O4b-f$n>R?Bq?1*xOWCbyp7JL>B0 zj;Y+9?%ulo8|BM!3b*y<UgQfHvpX7)<~ig;{LAu5s0b)LL49hZ3c;6$mj}ZadG?qeg`j8-v#awopRH|ie3K@njIw$<6KE*7PfYvCCU&6 z?_KfUi}(%z4k-{fdX>=jMZmURDYx~s1(PSx0ND|b4DY<=DN<;MQde;{}<0aYRY z0GYnuaf*uWkCo(4*72wz?DcdycriXsyEJv!BzUi#Yd-{M!)_eoKy&yY8y&?aev0{!sKGn+a+yCK^om^bU?iCJ-a%wWG~FS3v| z-n)&h%HeVSR!jNJcX`d)$BI`lm%I%b59tBnwci|k?d<8dh> zd^L>M(P8Txzu8&{V;lU1E|h;BmZ&00j)W+uAx2Z7gjCfm5zQ2#Mi59OPgm#z9{xOk z3r|ud*oT5G`6;Foh0sW`f|~deSVDsex`81a^1Owl<= z9*ww}J@`%TV-J18kQT?Sxw?ePMf>j}Z0um>DxN9GLw)|39FPzc_XeCA$RxB6lobXk zCV_(3jT4F|&RmiMS_xgKvS??E3!w3q-iAqJiZJLP&|yx&v-HN80cEEE&2g;I=xU}z zdr{Pf9JIWqWQ({Wl`O*~tSL7R(A-_@$(>?9AExl!M>QQg_38$B6dxLJ-(qucwO+`B z$(|&>v!<-PP-vQC{27gOpS!TD{Ag^9F*B3XKH59+AOQ-w2_LmuDA$Z>DwCkbGPEj9 zVZ!kq<8_yO)@gVf-avu?pfOL{TM?fiTTo-WKSN*#8}ePzfp89Z=s>z+eVMrbS(z5{ z{2jI$SHAE)w0)ukbp%H5g@20Wg-pZM{ZyYtr@906;epvPCdWXQ0>xmb*lZ zCbwFIf`VTLI>{}5A{`lY2iFqOs(r^DVtAgUzQ0;$I|Rt(1fB07pi9o?$aA$JF!xU4 z?P*f>h=yY;=2|G>IrxoEp{wEh%eYO@#W-!h!1Kqj7Al5vn|8HO!f(->#X?d)9PJud zpeRt+!F=c$q*1^OrGUrP3`2`3~O zGZJA-_sjR}RP>!0V^YwlGhSqzd}HQv+h_tqG2=%;fa?mFQ@WWNWcsO($m7%(*N^Ih z>R2)lUK9>)T0s?vHlY)4Q32STvan+R5U0!F(Xt{=({}~R%pAdFd@82SLy#Y?ES)j9 z(XOL6%ui5p-=#8ToF$WI(kDY=ryqdKn=GM?it?-N$8Fl=V3ZQg>mCuJyBXVTG$>VI zl&35L1bk^Go19wssOuiH!h1DmY{s6LJ129u z5pJJcKs<9q#(fN`?16B&Roaq3=5wFaVT+9Ih+cAUpM2{OP{l@4h-Y$;IK0Ap(rHzU zDK0F85|AL-p;2?F>R4fW!gcTYNa0F6LdJW38Y5GFIyq{2!qP-t2QQkErQo2=f%kn- z*_Jw(yUPk2=`sY;)}^i*u@dxK9JUxz%^u+yM@b!BIQ!?VdLE#kr0 zMO{v*{q-@Ap+R`?&(KgNkQH=+1Z7%^GZgp%j~on-j{%0qN1?oar`%W`k$L@H2?BvY z^3p0&RT5^Q|2YhRhll@r6af~-{{T3FGn!N0I=e!zRy&eMW3BrI18FmlGI!>R$^sarcLQ(zmp12q>oUMOQOEuK`&E=^rhw(M{Z<^F$8yxOeS6>Ql4 zzU>VYrRWco8oTddRNU1Ud=mg3lLYTDHvyXCd=Xj{EdFK>$N5TZrPHzOb)EJV?lqj? ziUF67W3nGN{OMOK^0C5iEvqpL96y4D$9~RL73+tm4aVs-H)c_%`)Vi9E|-53G9MYQ ze5lO>VtPpAm4a4Rt8N&6xI8m`PsN($^9-%bNZ+#=dG5^CO~#dj^Va~3<*}2+pE8GE z+u5Hz4*I=^tVmr(MSiZhvA6-9Dj%sxKg1IUK1Y@Xh_#WgV zNG!VK;gGyoIf3Hw)$uTv#tMUpc z?mYvLdc>1Lk7QnsK9TUgb4pfsie_I>lT&sz3eP7f^T@2o9(r&z1x|}AsQNZF&i&*U z4nZ-nk1!4x&kd=Auh4G!$EF%P%0Wnnwg!4E_U>*sVktpFz+rKC$(w40>y+UdFXI%NT>UAV_lX?Yfzk)OCt%^#)>cAY zT`G?w>wY1r_WbFy{~HC(yB}mFchBSnnjjS#RdYK7qg?}2MhM-}(oX3%X61Fj#5jwZ zVklSdk?2$IbupmYR_4+&9(~g-il=^}WEw?~{gHB@+|lF9$Ez#v2GghdN#IO$Y5XJs zxwUjxhRW**%UroccrNZWcn*<~p%NE-u`$Rl-R5OiB^(3(c@X}YEl?>%!q21IbHAuF z+56n^eKY%xFUWgK7QoO{Kbs$JyW447QB@@Oop+(J89G=`;L-8vh4jXfn2&^5g*q zhMyDWPaKd#R+@7x&^SX+&61{a+!m!!Qir$Ys(zNIBtFGOG1aoY3nNE5#D~S*(WzCm zYUTK1*6#Fu+igv^d)d%F@-fT~q46}x=-62x=A`*OvEZA#^p%75Ri{9NbnvsX>pK3? zWyqE_9+Mf>@2gmeHA+{nIW_r1?3}A1HQ#9T6#^j6s$h7p^-na4_28d&Sv`khwwv)< zs}?pEA9(8Xsk1&f`=AYc){x@Z;6)Y&w;|WD1U`$@J&6l**00H2cCsA;YS4KDrN=n~ zBtyeh?2E0m@Q3lSX17!-a4-<4%QVP|=*|dX1yKA>P8rsi3$On1m^umMXI7^H^e93U zdW-;u5zD{}BBR`u+0v5r^#G5tYGa>=qTj+zi(NWZ%!CN_>!hj3Zne&aEYODLsETi3 zk`y#nUiW)3dohbdz%oyJLIvB6ACmS1mbs{YF1Sk1P=w0;t-+2K`jP|C$PDMLl}t^0 zD*d_V*VIbMywF53c=fPVF4ao>r_r=-=M?y5L@0wtzXyd>yF+Lii(aTKVU*E~nb!bD zR}8w_g|GGQwk7}7sLydw)NGnSng4w!a(JJzG;kP&m3jsXsa5qaZ@T&VmW*kljbhO&u!EP^q?DRo}!LcdJsL2sYqm zEVJl*&bm?%%XY>(Bj(9_H=t*H+CZE{<{RD#5lP(T@9T(XdG_?irQH_hN32JWwrSr% z562NsV-iwGTF%!Iu+?%#tG;!zaijakL5ZR!C2m@%+}`>|UvLK^ ze}sSuUUoN3_WinU@g>{~a*w4JmpD|&nwke~Tu*72$5L{HnT+OR!?_YnXzzjZLeuF2 z5#hcwYPS$6; zLM&C^WB^`UeOg!`!=N>cxB%AuE2M*xv%Tu;qzU5Y6%|zz4S94yR8ss@-skA3r1+)p zpQ#Fr3O+Z?QBj!|1l-*1c;o_nP7O5VyAp0oj_MMrk<&A`F7i@Gz7X8tAB#(2zcsb$ zKf?i6-+|m`N@5r9SH(K@&=23gJEzMzc|4KqXE>T(8;Y%OMN)h;&?3|>VIHOb>TJgMSyVnV?j#&LegyPUtG`2JF^#?+hjiI+heB;91 zheP^dtgVno4?$E@@Zj`z(0gC8;VX=Kd<~6FSG^3Z&Wqf4(xrY*q<}$16`1b4TJk=| zY;t2k^d+5}s{0@Yr1N3`@lky^?&s(wVLZ?0x895!Nq=I)HKuhJo_pHynOaE=e4>bG ztJO&yWo{g|MRDKy(4CTVL3hhcu3ew2>mYmhreRaf2U&Mvrw!)E=+w$r;b%q8)xGSP z0#K2}BkSh1hTX}#301LBeJkJCJ?i6Tz@~xc=Sc_+V-OzFOF13zg3Z5{aj$u`sl0() zL*5zK_DG{~NmKB%SCwOKC!}h4X|LD4mTk56=KQ=kv8LsR1{2gEfT%wI+Cz5rxqe(8 zV`c;WLktc-2i0;{Av!t3vLZrB+mcO3mT)e^yke#e=p;ZDMUf>H1^tHnAHFl#ND^Rj zusfsHuglznYX1XC@WPxV0ip)MHWz~JR~n*;NuBOCW1(%3wiXq*I?v)a(b3mkt8ZHi z$w@5=n2e^Z@KaWd^yH1^W^$z!wTZ8 ztf@=*ri&BS@d|>ZsMUDS?}=wCaO(c&%SdAiYxTjKNp($KX&8s@;lHfz?r5rR*b5N# z(C*MGpuB!a4Dx@sySZfiBobWr7ZS0>J6bm!sz_LeI;?}4nt`PgXk+d1dNl{(sMkd? zr*YUDI-Lk4jjj?141liVIAb|MY|JYM@*kn7YT_jQWy$Evy+V=@7AP#y_3Olasn6tM zO#e7(5v%Z)`^r4XML59umB9brj_<{D{i*CmF_I5T=r7qQLaLHeLtf1lx?6QRQ6sR4 zy4Mw>KXM8<7AnZX46XAiG*yeCbteKZOjFDNV?lyXw)|OQ@k6*8i-<0JH-Q4Rq@8a} zL%nr^YgKS(;+wf)$(}!{IPx{jZows(Q*(&X*;2bJFeKdBq8)AT7Z^0E=2zHh$Z01h z=@qEiW3UfOcr6uf+;dFs2}oEa!%SkFNh&@)kSb9B$QD-7^&{4+kUH>7g;JCQ4BAlL z@+Zv6H5@Qu`$%BYVW4x7X7ct1Ck zU?$?i$r>n^m)~G_(*6CVW`*^IxsBJs6jNLsRg}sF_7%LJ6Bry&1g?rDeZ!mmw0n4L zzU0kSS86|o@lt(8X{r5_P_+aJWHvI%q_@cHNG-*TJMa3BiGOMyi@B&8Udlr7V@ zOETn@`e!+oCwg@>ERUOi1*L{TN3+Y&q7 zav-BFAn_M7!;+5}odthF6)+r|(oi;P>p9|^o$<1?6edDmv*}FzCPi=#2>8h(dDZt^ zblhexpoa5?G=6n7Ka;}9TwN5(LF3Qb{dO4ux-XNrJ(S~`u`o6Ntn6$@*kd_Yszgde?WLP_wlViRYQc^78}n{Nr@iOS_4T;acEP`vO)-R^=z@n~m@noR?Cz zruOh%#8^D0t&&%f5|L0bp!yVwgX<QnIoP; zk*R%HZkGuxfl41HwG9H<&fMW$*8Jjru)&vynO??p;Lc0oyd|+@{29*sna5L|ms@|z zQ}fRnqWlM5d9Y`J8k7cEJH3X_+iB3M{Y@2#i_)up+3gLn#zb}(;62Q+AvY@K-b1!A zsv0K9dBZ1VSdtq%s?0r;V5HfH%7oR8*tDZ8UGyga7jX_zj1Rt79QSh=3pugp6BPS5 zvda7jM(`#dvY4Hio!B8Md?8v_UisH4*j^f@1IRSkf1HhDrctO&tC{zuqIDDEciVN_ zbst7`ABsmH1NGE4)&9>58ews7ckJ~kFmsMK11p|fYQw~s&Y=<)nHD7s^mCt+( zf1@IB^s$qOq5g%0ZEHDOPkP0CgF*C~@p8J%e%o%=-g~&MvWK4n*21)nl9EXtWkMRq zf4=j6!*CN?4)l)=OnI?=y!jG2FjWd{^eo_hK&cnx9o9|S>laB*-rsHSuS zVK+U1}%#6{Z)i+L1p16=QTDNP zG{1>cR?_!ND3Mw`1&;@h+s2L?X8!8Z>dnvowLWkNd~9|#aGzFI8{xGH@j`j{OW@p` z7ZQjuKh$pZLYbE8^=$9DDZ>z?_m2e+kX|FE`7vI{#uO1uBTfMgy7o#({tQ`#22`!a z`I8Ob3m0+V3y2KqAO34(8MLRNbu=DpE(F#sslW+R-QQKU=0A`CpasbaI;}-&f^e+* z=A9j@rB+QjgKBbNJt_0@66`WMjxW?SE?Q-R+Wom2+4nag7DlIB+SD|2P6E zo}$&0zzAJOkgC@ry=Q&^t`rmN?rhri;jr@-#wE3Gj-ci*kB&8u=<2SSXzWDcbj{-t zX|q%<>R(&Fi(D1hznR6NnO?!!*#A%pqn%= z+%{^>%Vx7@PH;(X3lQN1$)>mEXdg6LNm$${bQUIpy4BH{QiZC|Wx-t&N@ex+t! zftd-7EY}&@HfCj&?J*Tpzw4T?EK}7PSBenSC(j}t(wjwH{2N=9f{2oNRQhoLLP~Zz zqRWrrw5Fm3s3HDGi0X-&!d3;&p`A<}Z3}}iO@$S8vJsurWejbr8nS!gB@BYl?8bg! z`rO(N)>6%90Y9+r(8bZfG3bE(0Tqd-A8l$zxP3cw^Ukgf5mRtZob;p@A$8=JS-Me5 z0108f7z(8t~y6;|7Z`VUC58j)||a;F{P4Hn-#n?J(mz~^!1ERD3qZX;f$Cf zt32I3H=i%RA)Zp0w~?jH-ylDFKVT^*s&=G#L+}tjl6SOuLvXHbMHOhzkVRt^Ql?$3 zI)?u96tpy{eygFTlU7Ti?v0ny|EwCb4ymAeNVpn6;*=;gw?xU0%2A#9V>xH0WDkSK z6`xy>&vjLC0u7ULLzwlg@dV#Zu41+`<+b4wA~Gt5$D?xz4PDONi$en_17Eve-bhX1nywM1cd53*G<`z1@QJ~=Gxh5lu$~2Y!qRayP@HT z6ob!Sa8AU3>H{0y)$1w92;^*i-8Iiy%ukRO`bW-QOo42K>u*X~SrYSI_n1vzU-;hB9(_@b>(JPQ#?Q)^0aNF1 zT%D>XOrvL(f>-s=D;=tPLG1+IU8~PoNaY4ucK8#WE6~0oU0of_BUf>CZ?cIhE)HWh z*D33(o|(l#uoWji7z2KEpRzAotWpIb77x0Y`nklZChkbv@#K8R*EPca+_rq=H!T*8oVXzs~G1kBy9RxnluRrq_z`mi>Rkj9oEG*wKDeQES)l2B-W zY>muTUI=*ga2h>2woH4*h$=-pw&5vecYc& zy-VyI;TCj#YpMKkE?m@Q-Q$V+4DC&`Gdu9KN?kDHmG0)sjPzaRdV2q95jYVH;zPMR z0%(JrS4c+QEv+U*BWm|WpVz9Gx>n9vMPMbkj6dYpntFGTaMufkn>9cFme`h(EVn!G zs&5qbUJ#U*2o&sm{9YUmp!!oQZy95Cu>o~<-W2!FJkh5u>1^~}ruQiKA;P!M-U2Aw zC+-pp`;z$2aMCZZoWcU@Kp?NK#>e*NBymWG0}e33B*pPUt?VoEpDB|!Ccg>Hfy!Bx z(?fI}LgACFX-csobL;`{Gc?TB9UK4P2x4}(XS32dbd~-S=R1=*YNLj`!LW|cGfgAw z<1eZ{m~;s&E}d&1l(V~j6pet;Ihx)XmV~IzM(=~FexFY0+adgh0Nyec`P38*5R})- zKLyng!Q4(G?GZ)Hu9Kmh)BGlrH$VK*>>MEiBXuo25Fl!KIRi5+nVbCyk4>5v+E7n{E!zTf)}9+gt8jalH);yRKgaPD1vQZ=#8s%yJx-$tp?w z6_J(F8_IQ>kHEWpC&Aw;q<9!a& zx!#NLol^0~bb$mtcu7sUsFKQ}xc0I5d{f1TgS|rCSZCeii zSCYMF#&6#j_<*IaU(ha-sJtX5uD9{-Gl7D85VupUtzzxZWFHH?ro_-;(eB1s{#v2h z>B%6UuT6V?{odMF#pa`^%_@6jgKKDm*-;*%sv1A>AnU=B0e;_i1-;L*I;%o8>vwrY zW7J_qNU>`(@KTpPd2t3wP*M*^Kc(_huHlj`o}FN}%>YgWqC%%4hVVPRBR@Rd&lJ+K z^=ydgbNbYihMR|Sp;LRB9a05$_Ap1|pHdsa1SV2IL=LEcwr{x90&AHwU|G?kH6tx( zu;QsR6Q?w8+ecRInB%B_@Uu|-x1meTq4$IiKrx8ncqXgpP6!`Y-ILyRH=zKJ2j$wh zKHEdAA6R^{!{ea!pG4)q>@n#SrC*Z5QvMyMc0ki$HZ7#Mlcn8}Fhvl64=tZ%m5{kd zYLG1@{s0muep=MW{1RkxGMDUB>Bm4X9WR`;vsr!h|=r?Lj`^f>m8NN*6?@}3XH}!_~z6T!ZXd8LkdIN z_1F7vM*@jgnA`>i&v&R^=tg;iI~kg=oJG2ANi-jq!h=}X0}Cv6$Z1H!R-)3?{9fxv zL;!;m$@JgwC=b)Q9bw4nB8lV~qGJZPhyBN+a6Y^0TLUkO>vn4$Cg3}pwO72?9N;+6 z#fF!MG#yDmCc~$JK=k$3rR?~=!u$D0UQ`8Ga=nsGbDRt&On=D;JoE&7d4xVBy(Xjx zs8|*lWIQ2Ub@%LF*O-mIRH}=Cim54bnLw>eY-yi*{>^~Q$cI&OjueG%wi6izD0*QS zB$;(7OgDJTLX>n8 zN6UWSy`oyR7}HTkTd5ZEZ9=RHVLxT74h#fngvPpSLJOul7vnt$_(cf)+W9Fqj=c12 zR<4~)DMAT#vh&TT`X}!kv}}dblXV5Kp0WmY0d@VWI|g|&SGMCT)1uzg;cPe0I&rOR z#FO6#R`0y*4U=^;SROgNbgv=t{D^K(uYN)3R@BHTaD2!XE_URby=q<_Z|OB*}zYXi$~ z!De8JHV5nGoYEUbq(UF5SkfJdS5dVP$(BK$?Hy)Ed*hF7fWJyo9Pwq#z(BgX2f}?F zGwI!r^rj!trTiGl$R{RY|o?smg)Tr`Wq_a$%|&28zm(r3P!@c>p!@$OGe-G_mFp* zlsA2Y?KC-IBsZUa%Vc|g9QNeg*{3~NKj&(2P)qnsbB&{=(2gL7_O^;NVEoBcKI+tPDgMQ2rdJgIR}l$S4<>!^rfC{}t;hPxCb(3c3u2sS05IAA+py|o!l`YCQ!>z1L-!zK_M$k4*sjki&p z_H3s`aVJcW{Cv_opFZpcIoI%Uw5F^-7_UtDtWrp8>{p;5>B)>3FnD?5OElqGtKaOU zXSuxw3)~>|CyrPemG^u%;d^p;u}H9*liPM!00Epr(i{*)V(`P@85!QqEQ#F2D;bBN&Xy*vDiw7AqJ z_L+u3kM3NW9n3Ykg~g_(pBRZ53BR1|M#)nVHu^ps^t)?} z4j@B7_3#jT@HnNNyCLJ$G{drsx)wP+4~_za{rKBRb>wltq8Z$U3+(A|5lIw8bSh$& z=+1ulpqtbP3C5ghGUL3^ejztx#aE$=7T9BcGd;ZJ$(p&ew0!37{CtT%f%}l(^9h0ma)8djg^N-(LP;w#UZm7QJ3@%N zzUJpeH>%+sa0dC_jcXT9ek{~=xvR#$_GBR=NMxGEx6o1ScvhPh9+2#-G^nc|%nWP2 z5N<7(5BVojf(m?GJSvoix6@0|uLa9)C*i2`wnT{5+%*Sgg1~YoIwGDK@ai6?Jr!gr zZ!bbY*&D&A7?T5xkq|fht!fMuKuTz4$}H)^CQtN$utj?*KvsCLg% z>Rq-KyjKIekGqjp7{8WM6;H|q@$zJ^Yo2LYI$;0x)GzA1t;fVi?o3`U$7wu60m?=a zi5D`(|1=g`xnIWEa2Te4%_7FdM@II$%l4RD)$Jpngbp&R*@{o6=-EMd^&FW`wHuUt zT+t*@GFMSn(+aLWMA6Y{{>6rO^QCn*NnUo4n9(<;=J1O&v#a-!WW@7R8hmU`51q!s zHZMPtg?O1S(P+715A>2(?&6mh3yE0bM8IbjU{5^LdLWiiAyhD?>RP9Lb6g+3FUIv- zEY^88>%=pEzBd)&)ytMtnty&}W_T1acx?n8N#v`qMS`+XI7yN}KD0OeI%&7Baqu+u;p9-UbI=>>uKs8Z|xY_&ZF_1!e(J zGH@R9hPFgO<`ZET|9WVhO0K%mrB0^x?j-$H;)gb*-(~u-vbw|W*2LWB)g-~N>Rfg4 zS-OIxa>4&AxRgtlB=Z(Y4UYU?RIuKZ20SL)Avt&Z<3>TZb?UOeNi+aUdtRYeJVuRS2fiEZ!^sE8wJcY%MtjpgB43cHv>I5KWiG_gP)HF~ zC`dUWy!tY-CL12m>#EyoIZ^snE+_u6cU*VYl;70)RaE!JvlWt|7=+?{YYTW zN=cXiDND*qx%X<+_a&*k=eJU{JlwplCRWB{AB6FqItxRGbk66#OHtBOKwdbRMRi_(HP}DR zT?byDSpV*lqag@7R!~8hAaNbge{YlP`vueC;q&M+r36Sg-RiBSqM%Utv^Y2f*tiAQ zxLDZv!LPD3(A>)09AH{GRwynZR`K!Aw^ou8Hu&Oy zYOyO3K|Vr&*C9TjKtN)H{%@4^-vTg&G6U-0NBCbc&j!kT`hQjb+dqKxf7}88QX?S< zWa;AUY3=OE^2y0jP4PeN{?9K{QvWk#|3(b?zYNn^!Nf=BqPf`x_sN0qGk zSCw$W|IdieK%V~^DF;OVM-~Q=CA9%D|4XI;{VV?ha{Nah7jnIMrX&|pFLDAF0FZ!{ z6a*3jW9iCCifMXfpXGe9B9O}+0e|(+Dhu@=`)W?8K+Y*9!|RH89um)(l+&E3zXB0t zX2Bc}gyT-K75jyRD`CePM8HOxV~MTj1WZDN2pYs^*G9pkmXKWy8S*vm6!R=kNv|Wh z{(tEIC2M5pInt6>Ea5{{wgx*Y`jhbqY( zp9H+^BuS3uKENAPPUFGKsxrb@Z0qf(Wz3M`jEosb<+Iuqo1(DyT`=go;>V2$JTsh; zx%rUER2GjTzh2UB(Qq_JwdtNuU)bQ0;d`xa!K3@(kI5v1qRRYwJu0L){r5kAM*uV_ znjLPXqw~~%Ua3M4&oQOSJLjgQY_YW`ePsY&tz+}C3kY;Hxp%H&^41>X5#QGIGUF{X z2C6Cc5Hu0in*9n$mAY=A6aTaA6`htDsb2B91Lmoe(sD)J;EyS@4tw_UGTbsXz*lK2 zqJ>@6I{CP_ET7VYsA3_VDk$Bb5nRWdBtopooV>gMw@d*r$$?P%GTw7@wBeb=G3^O% z-kNmuzkgQwp~+iDSvey2CoZi_QKaT)F9H%LcVvOgIonW_d->#yex0J0z>}h|TBPLu zsAxH@LX8e%=c5&@+0Zi_qbJt6`2KIpp5!u>x63z5oTan9;98zo?$Q_C9^@zTB~ByW zA+}tcz$K4E4}{o*lxz@b-(Z(FK&}(46!3BT*stc*gcF=M7(RGN(-wmRr8_AS;x*Nj zNg5IB3o=4*jI7p}O3ZUT{lw*+`5IprZ@VChO2G{qh$zb#`&IftzhFCj3^?fe>W(3p zx{axe`$Gm`Lj#x+gw`Un7kM;wz{R7nI{|9W5l+4JAub%o>`_E?Eek@jXC8r_Okg8t zUMBbG8hO1?jHsbc^6)uUqu*bjHjj2*E?zLw9KQN~USeH0&&2CO3m^JLn_`v=vk)~M z3mSpVYRjxjDiUXFU+mNt46C#WAer{#WFs~*RyjV(-NL1+9CiYO3-W~@O9GW`n=Mxh=zOUYk z3uSUm27-wiz5w^4k*}?3)q*%tSCUlv#<(w~au65r1vMA9$7_>^5yJV^{nkh#gI_+L zV*xArIRPJ6T_3TR0`Z+QokS%vDtXw-BE}EOF7gsA!fJYt8=+u5H?}5E+Z#%}*ozU7 zO$TOr8I`nAX|u2k=6Db4)fpObYaJHVU}AM2xWIY2McwI?i|)B4loj_xfoi1_V7(h1 zrtV5m#vryWn$%doyKGy!4MMQR6)JC(O}RUd3ytN8?BjH_ji=;k}g z2^7nJO_q-STw7fCN8lt*^IBPJ}Dt7Z!;97JHS`ESS@f~Rm4Uz|5e-D1*b6P&)mZzwI@lPU&z%{YEMRbx@<6t#hIXt7xC?=`L<%x9J2Q-= z+Xr8OyX^(5m&WQ?DmkGgU$FK8?! zrt=;)z-gMb@kpQ&yUC~6ffnG1M;a)`;Lj5Hh}w!ELy&<%A{-va-JN`(;s@Kvz-neA z1qs&E|Ms?}^24Lj`R&Me(C>@X$K$=!@08m|UoPoibdM_PvMzf!TXQH%G6X#uKmCkf z#p!=oA6+eKv06^)K1ki{G{mnrNrlHNf*Q0Q(xEml z+F~~emDqxe>pu0DG~n>z(Xn%xzvL5n9*-8F5Q27BLNuM9rFie(Zxy9)jxaur377z9 z+kn1N)Sya48K1^%Mu_N0h9HgP{D%HUrYRlwh@`Vo;7s2bEIOg~nA96C1=p}a@eWLr z%>1KzvyMiEP5^SeQU@5r#*~|IKHu<6XAz0BwZ=UN_3F0wxpCeQNjrLso=*Xbz9J9% z0;6LH6!R=L$^H3ZaHUc1WuV;YN5{dcTyP!+q!2)lV7!gP7o!2AvF`GGqrAx{oBIJ@ z;Nv1v>A_vIaC2Qw0kndLG!&Of#LWtA-PQV4<%DL zMHy+5k$cN;vkOn_Q`gs1F%!Wo<6{KZXI92xEHp_C0s{K-=_jo$?`al{-*%rMbI%sKAH^9YSu_}w=;Y;+OKx~Rj|a7()tdmTl&AB3mDqx0x5OD1?NrW;z8^W2-DW3Mrr^ zZN%+2!8*<--xac#pwAIwq{TZhOvdA;40c%=D9qc!HV;dkwq$CuU9A){QqiN~lelpw zwJm2Hn0E!JA&_Na@qv-QssbPme|KN4?d@%Wer|0#NcnUX+u=gsFvHJQk0RE+g;tVf zlC_jWzGkOeAweI?B!j?uQgzj{w<-ykaO1Cv8Q3Mz*jg}ZACEWFACEVlBA!~a($9w@ zs2>eN=;i3doQt{AE-y_GqCi4uj*;%qrj#CaZC%SL{FpFO8BMojl$YJ2Vh5}yxNsl1 zZ@SNpMF7RR&EaKc=nq_fn4N?6ub>|p1w-5VMI?@@0AzMmL@V%ZZZOCR&H&MjWoH)w zlYgGB@`^wO1`RWi&Z@{50JY6#+D1q&nh)r-Fi4ev#Ka6dfb`-U#0>;p;%+gA!@|)5 zDC6^4P`@q%;CnF-a)V;?6h1kL0lj*4+qIxEF#+f{s({Tv*9nTR-JYx zrHD(z%eX@DT;OdGEkFjf!A^+fs!p1&Bg5!W-8r_HuD}&id2Mt_fjzI1L9L`xF=sdf zoh`a-Egz5_6trpqq3(i2T0j}GBCi)opjA0PkA#<>*wt^PRx+-tz&S^}#2=CmApAoP zkyV&&glnINF}osLtY|7(!Z0pf2g{N{B^F7~g}|~j2TrjIH8${BVJr*+IepVC^eD;I z``X^lU~w=$E?v?j)*e}+ty!@$A+_ywj0xj+@dwpX6v*3ez+CpqG+pts%B)0ZyZ=R4 zHn_U_TD!CAG>t-V^FLHRM&r z6lzLenW#h2bf;V1bFUEPl~(R=_&R@OXfV_-6oa%wo?AXVw=mc2*oJkznq^mW(?anU za|^EABFAsjX6%(~4b5U($+0sc{&4WU|``eUdVnMwss;CQ4fB3c0Dd_EvvH5eo+9%BiDbq(};Q@Wy`>S5_tqaWMdG|L9D zK)j z;4RNq8o&r@wmuY!V_Nrr(Ba9mYMM2)zOu`+yNaOcvFgDoY# z2iOV;;{g^hE`co2G%Ji;6_leCPYj76Z&+e0zF6UlZ#Wn(e={%XjdxwzXOJ>ZZ)Mu@ zUw$Tp7JxF^`14}qdEgWwvn{R9mb-;D?pe)$tJ=TlT&|hR3cjPACyw8Mx5FiAi@?f+ zZ3@*hVbkhq${-KF-+8jX%P6)gfQ;ch!cHazkfxXjARlvu++*w=o>6;lUqc#pt7qyb zPvdC;{GqvBG(H|Ffr;Gyqdo}%r5d#k@rX$pzJ)l--rZuX2|6`aZL>Y&8 ztK6{)u=YN0_BTVOa=OsJXURla+n1-Qx)>akX}Md4ijZ3gnpoAKY8;R2E1fisgZA(C zoltH*`$HaVkAsH-m(!4`1l^*~-i{~0KRBX(u^*8U&e1}gil|qC^t*N zyWeX~ZOibN^15LdJHNeOokfA<{HcbvX|4*uerfwpM_2f?85bh2uZL_*p+dH_C+Dwl zB^=fY#g!U9fIy84wUZBRTDK*?WLM5xI$}MYJU-CB#Q?{Qz4Eb?)G9d_;g;_0){Y2;GmZ@Jw+OGLstk!HwfyxnTXPKWtv;*;BM z^oFoAA{!?L6WPb@gX{>3dHxcww&?XL&^Oc^$+|Yg8X&y|li-82D+U=$kOajb)`zAO zQ80u`U*t%ElyYjppL5+#Trl8l*&1X5hOM1x5ev4hyGv&J{9@c2*DVSGw`iE3HQmW) z^Ozm7c)_2`6meC(K~d2EvmEt3Lt9cDfjLqVB)2_K7?OQ;Lytg93(z1WDvl|!G)EN! zdO_RtqT~ZQAxR*-kl_JvT(kfg+#Xn)9eDU&y3EPSXZ$g@AzWc5{WcwDf=xP$L-?4x4J=PNO|_NT^F3aP6tqAZ0rv_9eRrA`Y+j4D z1i^;fogh|Kr+`ON1#A2h487N@CQy0F0k>f?U%zSy1u`{9d5%TQ@$y2V|M!6fy?m$R z4`i)ZdlXeO=mfN(DzqiFqjqo{LVCvQjnXS>-vFd*gN|3<4DX;5FHDWH#F%uGSzJ_u zu?;2_asuB{KaxvS=_#`!j+QLyp_bCp(67j^&BjSr_?N(bZ4GTu2~v+-P4TgTqsL;c zg8F@Qj1$I3T=p=YVbf3t*OcjV``Lj(Mkhp;7g^k(PYmcJ=-<&+KOvRhldMzB&_hc7 z%Pbd1j}PjMc3MD(1o8;lKPX;xCmHaIfEW%k78VwOgaZf93n`uyh)D@{AWP`005?r4sPZSZp>cxc4vlb zE<3!5{#&(1Rvt8!KKiJA0z&pWtwn-$SjL|A$(l18FY00n$rIM!b`z6X-Y{NTKX|^d z0~|rJV6|jL`BKdlB6EF6QfiDy*Z4@t$GPx>5(dFl>GI}$9^d94mrM8S zhXS@or#nZHr<*p8CKii#x2DD}FW$SZ^jB{uM#`xd)p&r?1}{=xA)*q1YJcDEB5>EJGP2e&mZzMXq2(>GLG7Th)3n(C#|rq~NV zc-p@bwhgqYGlbA~Q=Dm6g%lQc{Q*ME|hPEGlk9_0pS1&@RZD z&s36Www=q8qKLkvt?g(uwbgmDGSsGYZ0c+o$k8$#(k{^n{uL{Wb=^A8>iI(6jAZ#G z4jh$Jed4TEw0{(gb#7-VXTGVqyK-JiKE2$mzz|S=^u^Ro20n?zv&FUpKCNRjYH?4K z3n|CwU_{Zj;mm)FMY05Gwl6n=A+nHNUWB75uUQ)mpRY)GF`* zs?axWOg%WYgKu;&KMa}2wc)CX6&JW1Xg$n-I;RvpC&%LViAZx03YnShJSNYcuAK3Rsnpy9PkX)aN1WtOtcnC`FL|GS z+r@(&7tL_vN16wg>nMWqIob>l!5_vbdD(bUQfQImZhS@xrQIhO=TA}TuRN&b^NJ*y1BtbG!U7vpJJk4T>KFRa4Li9<3K0q3h5OO=?g zK|KTzYBa8C?yuhPdSiEKqL#Ld$;&#%rk}4bw#>f!pKN^$RE{sXUv~?6 z^R*&J%CFr!8IIi5+U^b@c1RLgr=g<$C=3VsTFcfNAUhCEzf(tU4d3b?lI#P9ZAqfV z`BWHV{gyJ=biVmcj)CW_!PI34PVwGOvdsK^8~Ys-pO8q4aN-Myf@GGj5TIR5pbJU*MAfBu+-*uU=nYu&csy|y zRP9*?e^;(bWds$QOO_~%FyO}v$2*tW_z3ZozPZ`?S$5iI&RNgsfP2>j^dg^fKEdUz z`S&z*JPFqC{B~8RALklkaUZH}Vq*lYI|jcjs>Y#UvIv+$QK+RpXkIgbbZC%Ll9hq< zafm4+uzfj~veKD$4)~MHs`rGGwXn8lSYew2sf=mEd&C91{<$xgL75l4!wTCKG)B#F za)j&#u9UIM2hL=V;7j_56i#cE4t`b0!oG@=f+3>OF^BZ}I~BB9GNQg`scd3;5Mi@D zE+yGI8nhgY%W8X0>YL{fVT7)>%bIw^A9x1n85At?o{Ra?Pr|;X+3&DByVS~M!x;a*M+yHWaI7oPR70-hl+A}f_!R&;04%hFL}E^Gw?Z_ar8ZM zyRUM@NZ-Y>msAEnd2Lr+Z``x&cDJ8m|9aMRFa_MBZH)!4j zrRX{@Psv2ia#J}l8ZfA>Dnw#kT=*p~^KLhYE)CP;4XFB5RwS^QxQy&xP-OTI@qNG7 zC20f;V$`%cg1a;$C>_*B_xr`k0= z5tHMa+M-i>BW+HjsOPDy!jf<-Nx}#bWT~!H~whs>17^%;vFrW zhRZp(|;%&}E;psGDd3CHjd?fgy!1HNP_KFBwSLxqn)2q{~(-Ct#mT zt9PJ4G%2D+TW-#RaaQV)z^IJ}EXYXn;zV1mwJ92e2 zj|()i5cs!>vbt*ytwPst;%97>W%!ymtBy}$Q*Yz$XS%WoNAA~e!MCP*oW(MhhRE+v zA0Il05l`sS8gc}g2D^F>#l^C0v$K#z8}kCO)lRSVoevuZ!0!>{2{JsuD(5gcSCiz* z@}tE-dzJQZ;$m!{2#~FO_k8Y4K@P+3S58XLQZQL`al?H4Y(ZFb!+iW|Mi^yrc1c*{ z6end~Nx<#-t!pW-y;~{So}%;Wsj9?5f!yg( zjmXjYP9t$gdD=#?VPme1?=3p2%b!JR*3V_#wE2Yj+Zik0E*ry()pT`=LQZONzZ(`B zA2@6#7;AZfMIITs+mGmQgAu}XIwr6N3 z^h^KQAKoVZUa^~(y5t2n$`K;${19Fs6vO1GX6O3pnZxpYnanyGNkY2b;qY5&=C^cD z)hk^wm;OtlCzBk(amU@Q@HVn_>K>)v54%3kfi1g^*&=6-D~@_c&5vuc_(qkM}WA3Cs{pCyNqTSg=H%p(6Ri;ri=o2AyK zVOrOV%gci8@o?2`%(d{3k^{R7wcxZy(hD>dnMdgl+79i7aJ-$|U4`BaaX(hMzBA{r z-b(DVmi~zrYvvWnieNd#poroxg(1ppr>ED25Qi3r7C&bp>$y9wE^YYj+wWI6KYK*B z*ODiquk$e^ar6ZPT_2b4Jl!FgmT!s@6(@AkQ#K@jZy0~q^TdQxBKZ6>m^AAfH zfjVdLmwGXljnU@ubf+6-SC4ZCQ*$%ZRI7-WfAM3#6hs-5aOgMh!cJ?F1{-k4cYZc9 z$A+Lv>9c^sGw_M|rYK~HV6gSD37+a}cgDJ$auEz7O<*c+$J%5%@|89b@q1E>5A77# zbc8-vJ1Q+|^6;Q!FVZ%i{9cAW%S|8sMQG0eim2%eZ`aG~6*RW8@o&s@#PrkMj4HUi zN$f|(27T^Ue1dxY-Pu3Z^qSrGrY3c`<3ps3 zYz>{h8j95^(BUNv5M6L=6MUX-m7svg^Aa-@5Yo2 z_H6Xt;{)U}t>Y_sC-kX@abrD+e3rATfyhAAB7y;T_s4Tx67H9bU*y zvrzdONr2uTw(c{VU}4u+3_(rAdK4%(GN!`MKmEX-ptdSbp7~lwM6TZUhUXnqbWYBO zMq}Qb5Q?R4YQAn<^ivEpj7x!`@G`aOmfYygbg-p6^YekR~b0# z5n|M3$wV?&7cly?nB`DavGt$f9IX*Fjis1smuB3~Gd~urNcK(7+eDiAN$i1#MNbm} z%uT38`uflk3@JBv%a=gmU9VKz;Tz!kckY}LiW$|P{-J2xZH=5?N2)aHgolpY{iXIX z>M)+`NThMTVvQ(w7M{4(xnHzS<=vl-O!BW@tsP7?2!SyU3%=nDBIk}d_T?cc{M7$c zv+dPKAKQKr{M-O@J1k5pAyM1fiW~WD;vuN;r=k1T&Hj;U2_AzU<3`{nB?MjlV1HJ$ zrh|ogC7F4=6(L|ddib53xVaU$g-C(M`liROA9Fa7@f=%+wbm~nPS9SSmC0z`USV|+ zS{$2|EC=RSKZH~#?W9xpe1AJ>U%)ls+f06Lm_I%O7U%Z(9nDn*Qm{dcmB0ac-Z)za z#N#9yso>nr9FkOJT!KS{WHEZ-)2>U&=d60*q1r&G$X}h}p?>GP!oqaD$>EyEFRJ~V zFyn=RAjK!eCw=w}Ivgl-O(9zqT@JBh4zv!m4g^~V9=-4OBs*sg!|8Xs0MLy9Wxz6E z*)*t329KT5Wn%Mos&4&j-1`(;Ljn}{>Gkls9gu_ zUU&I!!CQ)g{^dXx#Gm)6h+HXA7oT;p?aprlj7+lpyA6T$qS*h?3`6RBn2u^8 z><9~QK2(N|#STuGmD98mhJgqC1FsA`?U4Q<^ISDBt;OrYLQC5Fdo(q>G9zt>elD`4 zsdzVxAk;rBK;+s0YYjV1zsA8F*?;+Mida)6vz0M30uvAQ4`;wpVnJd}e(wT8lv#r5 zutN>O2;y-#AlWwH9|CiNnV=KF%`hQ~%msT&AaNfFVvI$3yh0jOG-LKn_)zKBY2{W7*kS_ zmD*lGn9Z?+^GA-QQ6YQXd1|g;3F(x~LEy=dbvT(3y+?4ual&!Zacn(N8h8p>NOt)p3${h5 zL!Apx7tZI%zlkimQMSA~koLN5c1AhFA5MWut471WsUixT|v4)o~qxlFk zaf+%KCW^KyLS9ZXfsc{RyGVV`nc0zOLK`n}7GEf6UsvH-&)JCXA1a7{%%1?8SKzER zzi585Xjy-h^9#9}yxTpe>yh!bL@r_HDVFdC>1W$@7?Rh{`LFilGH1hCx|&RAnR4J>}gDAI{w_pT3y&DTs^iV+Ckg;>Ye_7w7?rs zG%F-+Yk$f=6+Hls<(Md!{-gT4X@GGfMHR8}lmfAd0;%b|VrY9yhLXblk8~x-+J)LN7Zp1_dQ$?#x+U=7Pp7x?dqc9RjfI&GhDE!y39D!VKn3LJ~e|aCeQ!OCwMPfOd^{o-uSWF z&cf$4gk&M_cn<_nJet;%$syg9uWfEF?PK@jds1Iq*jZ)?20q0Ny(&>tMTT?HO(egj4;1FMcS{l^m(n;H8~;?fqWFV{TqIVra-s2L6y_i1yI#P+k=-(Qf}jNSxtlC5?tYAS4+uD zrh8G9n&o8QsWf}!!#4iXw&(BdtKDY>CFr9F|8w$az8+ECfx4;f`Ww|{`t<`OA6~JC;h}jJ#^~VgiZ|+Yze`h|tA4NNC+Mv4 z5fz?~$7?W{QbncUWXm^FUos#YJnrHJx`hW5dFo2{ejgseKM*|zm>shYP;^&I&O=*JGP0p zh@p9KxI0JI+yMcqR}bvlHPx!aeT=U_SO^{muDt)6;yzDS){_e|&j4@F({F|;Rd(jU{u`$ybc&SS-Afh_+Kl(N3hcVbV&8+ETD()X{XxW4ZV z7iZ(wKi@Y0n39Ul?Xls%m@loVas1SP4b5T!GqNf-hGd3)=Fwj>4ev^#oNw95V)52{ zJouqAOS#E)g>XZYWkKU|VKK0rn+2D2ed505M6#^W<5)JwI>k&A;x8Oh2UZ|lcWU7xP@|~YXWjfo7D|}!yUe`OD&qX#anHf0e69SpxOB>sP z58g=&6damitVYo=T2NALki>Z1xi8EO`=xBbg3AiBejTr86CZENAgbBD$+Weiqq&1Q zH#~s&{(YWQR`_7f(`KAj+Z!7LlNeubk1HIEjT}S4m@n2eibqj?fKQ*_z>jB!;@=yR zc$9REIT%Zmzsdrwx<}G4Ed5N77AC#Gw6_=+pwdGNbzZ}rU8>fWB#^;!VSxb0+kwG- ze=yxUA)A3H!;hT`N6LlfJrJ5malwHlga}unOHWi>4W#EUqKW=jfr&m0k9LA|!Wk9B zpGdY$V_V*jI+x~kNtnJ0J2yYiKNgV2p2opoiCNw*WJfBG%qN(pTDY=Btl9}M%1HpR zClvk81ziB~!G(=KzpXa1YsrWn+EYai%@LG&;~VeqrVSunkLd937Hiy?`ToqLm(b+k zC>sL|$~n%o9C_~>$h;$ka>lf??2%XcKqzKy1UcNHR)ziEbM*D~QQ~`Lfvorbd&V3C zXWksu)N7;t!F_ElV=-%ZWF{jR`^S~jw$UC7m`OCKry6SA;)L>DKb5ykE>xVlsUTHR zUv5y@WTh=1jvQ>=H6HblWVxS9-Dq#wH^$zDVg9V*A?AuIF!8X2!Bywnh=xM)n@;XH zB==y17 zGa(k+brCCqa}bbSY{vp=K@r7ilW6_M3e|o28hf|zqJJ~$XlNSW!R~qrt4Q4huexY>5p9xl0tFm%u=5hL z``{#1I@ukIEe%I!#5*Eb3@ED-mZUL6-E8^ToHppj!}j*=)1AKngF4B`1#(wR3aW8tCt_Be$ciyi)chlR+2S46|z9J877eLRuN zpWrS(uySK0b!IY2!7nANzEbbhQK9cX6eS#wtgwlKW8?ypB8>rH9H`$Akj{semU4v8 z-gWvq-ToHwAv+&ESfH-&vXL4**fqKc>$-Wz-^c2=q&0LX6?`SN4^_k^pp@$H83mi# zT%^ZmZRPCG^jl6_ic`4GC##*obM7JW(Wy-St{YO{LVx@2i)!>(2qxA?>B)Ze@X(k| z;FnUSgOhy=SyvwiC(u)P!Y?s}+@eR8Z%FlanJqcx6)NMb@V}h*=x8n_B&U_n%&xY0 zmIrU63sHZ`VNYl4S}oxen`6H9eLAPWqCjDL!rlEm5i)y{XLvtjLNcx?ZPlBHbVg)0 zdYgc_@@l8kBW?W?6c1%`-OI^t^f9z?6 z`TYbz^9LqFJ#zQqZoX1vnV4Au0w!O#@KIfQ;J^<8I;7%TOh#OP47tI`!&!XWdb+eg zaN@bNqzgF}_-164&`CBYI#;JxWq`}t+PEa}B#dDeE3vxw$O>~TD3(H<|1LN1gws=6 z(uv}E67scCQqS({&_oK}Ace7RxvDKexwV>_3Q`@8UngGcM`9g);C(W2hYH6N>qVwx^=dP2){{h| zxJ;{~m`Q6OJ`Ssk;A(BH*vAmWA4v=@U8|eA z7=(4rM`?8d`aUSVC=)mkJnW+3#CPQNW~OpwIuL7q?t{d5$0y#M%aQNM56{qSKg8Nc zt0a)|!Ca7~-zhf9e!0sg=WVVFwc|3LqAj0jd9K9JLFfG%;O0mb2Il00N$PAbjVJYZ zi4yf&Kl#ub@@mL^ePSW41;q*PthpBY?M6&>!k>az~~#R;ahA3W>kxSu!&(RXA|70P~du?D%~1~ z_GMakGz5-u*;H`q{!D>ogQnKLM14Se5Ls?Y3?B^8&uj}s&{Z>IoW^CY64V+KEnD%K zWFm7&P1(3RYptN<;J7BrYEqA1?0Mp&-(NDb?(x1kKDRz1`BnisQpXzaJEr;Mm>?d=Fhc);E1iU zQFQuf!0R!pSh)8^4UcJQBN8YoX056|Xa;zyF_r`D28ax;s}!NvU5u5lTjF$2DaK;_ zD3vk|)1rdzR6TQc+$*qQK3{t>Dud>awVYn_Dvg@#uO7fBc;J34QXx$>Al}hKTDI9&n zUd@q!iCyJvd)t{Fji`&zL}(`ffhX~mjXk1t2X6>>u@m9uPS>wzoTT3$JnA|fhtbPE zJe@C;1fZ^QU7oGc6As z;)-|*;xJqjv3Q>&9mf3t|9Z3>0)Sg!ZPd4y?}WkCFN z@D#%m{_ScJhhuf+v461dT*%xtZisRB`-CZf_c+Lojdp;UQ34+e9HER!o?#$jzTekL ziZex8${rzyVbXBDwCL6Q)|6`omma^SdXUdu1K}ZTz2)?2kIUYQ^-HXgz4Y^mo+@CA zB6vX^WaI;|bs0OonpHVM+fjOh@Xo$=-O$hDhQ#{v$tu1&6&8%SKLmWWmB^B zIL43GiPkY@V9jrfAIJwi^-YQ(56JTVhP{zVbzyF ze%Q8;m5eve3_IatqWYSHVx9KclvtYXx8GUi?b`fAj=!s^`z0jLk&Wc;Iuo{%K1`SF z?`s-4jlmZi7xo(;a*3ysosU%1$>z_pQ(4noD)mrpIHtmz3nT=&6=dI~Q&E zRTG2hZchGOTy+^Dgul#*_X=nAA?0}qKW=7}-Fl`s4Nu+O6~l0@7g5Jndnr#h3Q84W zebczS*qt@FZz1vEXOuD&hs8CxO7m1+jW@~*x-RAJ7!u>0qo`M?-UaLqSLb^srdga8 zw?6J7=5Tk~EDyR9sS}Q5T%U73Vg+Bpp)lY-+xHtR?wiEVXG<&}pugVlvBvL<4P*d7 zNE`6Kz2AmT;S5;VziE&+m*Cq5LzyTDByLSsE`ClfehzMC9=4=Ad1^>DB`&rk5(O3@ zN0N#{HjpjpRDu0(4rK$Gq7Lw%WJ+J9f0HWflqCLXKPajHQ!6UV|5I-%6C^b%Vm*lA4rh{%L)v5F|;e(Epblt%8}fuY&!bBS9~~q%9y}QssXbf2c_P z-|S6wRbKGlFZrLY{@O_M5Knx_n(Ve1PK8AePI9Bj2e;vfW5h!@&9MG jBnLGD5TRTWt0E4zssa@BUprI)Du5dj0C1E4JIntA8}Q6n diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 3254d6140b3b516870c230c610f90940800249aa..70e7cb246765860bfe11597323c65bbe58a87902 100644 GIT binary patch delta 25391 zcmV(pK=8lzvI42q0g!!vfCV{2QY0l&B&Aw&rs?<7ualXc%-K%a(>YIJ4h8=0U#Gu+ zH~sfS6aW17?~~y_Oz$TqngPE}0)zLh3SmJ0`k&L!-xW6g`P*O1D0C)A^E~}te(`+~ z`pYTgd7@;`vB(WMohue{EQ(N~b7-(R`{`OSKc8J0p<=m=tNcDO6`LRz(Df9fYrlV+ zVhjq$7|XxHTVa-Ztr$jpYZ&VSZfK}d0be^i*yR%aD|8um}|_#_p8)P4*fR2ZPC^@USxf8E87hx3JhW1fVXeyK3RF^^XDON~JT zJjihxPgD;a)EEngH6tD<(Wmo(bt4|Iq4$7IBOWNbrPIyPhzFeQJ>Y!PboXrNIC9f- z{{-rI(DXax@virP4^7YgDx>p&AMkT9z5T(C*dKb1IAVWDz5QVxu^+Lw@kBRbfAqcm zabPva+}odj%!vJo+6tEXV!#8b&Y$M=8h<3uEm2{CxChr^N0~KKCFut$Ht)tlr9!l5 zJam-vGmSs{jw9~I^Gel?Va@J|QK?9CdBNQ7sZ^;#Z$GlAx9UNg76?5^u80}-2X#fj z=^M1Jph=@k;tYpRXq|UEYfa(x4Bx3J^u)Ij>wlkrI@U};Y6*Kt&$V@>$1XmgC-`BO z{dh!;4nd_3$F`yiQR(S?EZ;2F`mCBAbyF&RospxCO{FK*?WiZD^e(AxM}4PC?~d(t zcCpu)V|#60?6vXOUKo$M#ZQ?4@vQFM64k-X7aaO**o?18S3WMM{2k zXf5)8y-H7a*52Gepz7fYWxEQ%)q}fGdNv;iQ-6|@F9u0oGm4U}8zh?156~+l%BWZL z4yj^t;{iQnfM^(t$>JiC#3UKyH z1@2Y^>m_r~ycYunQDNo?97t{fdrDQI%+|zz$6S%)NP0jK13EJ|%o+I$EpUvum0U2~ z+bM=>RrmvHVv!8^dU6@l8u@v4_gB>Ru9dy}sqEILz(xG)CGZA*BG1>r1A)8ktF{p3sWB(V1^&*SR6!lrmvU}-nMHH_Of z%qthcB2(bvTN@NE+S%Ct60BVOGH-=5nmvN?cyA46J%n5IW2pjNZ*16}Js)sbWWT;t z=R5Q`*8}Y{rttMr=Mk?Yx=Z$7Ik=XeGF&I}`hMbw`}@>fZKfyQ%I^~<2XH@sp}Uje zg2a6|6tKFvj5s%#a37rr0dkywkU2RF5Pmz>mTEuf^6#EAL;26iOPZVOqTT^K78cBA&&zm;E%EVYn~}Bf6X+P z1FCS9e_r?(A1+VnnLaKQpHzZ~cTGg!|R2xzh*EmMljm zPNe!YNpkKk@3tr!Nb}Brig7+^(eG(6X+Ef!q#zaJOhEb36;sJZ(iX`MYrjnLMx~Sc z72#JHJPoV4>b&SPFIuZ)tNCqb&uHHLE!)49cL!9=^Zm*p#9dp{ZO;oxeits`QO>;; zRU$c}~4`b0%R%Wuh}qz|jB^$!miH z5?zM9B)8ni0u&S~nUix0my^e3g@bp>B@k#EjQWIOj0`Sl16f`k2`~DN)mDFI>o$`1 zh>Ie}N9x_W&sf`XNWMtMj(BG^*!_Xt0zashcjmYnK_5;$u)EUO$@|wzC>@h1FZU~E zai%PEL+{Qr%+*F9D-c_=Z>2z32=gqLKyo3Y7-6=0N>yPi*3Y4N^>C(a^Ig!J1W7Bdg}tVQqe1 zdqrB~g+r%(>GIvaJA-p;sZG@8ex~Xod|7zyhozw;N(lH7E)+JA@pzf2i6*Tgem#BQ z>`#jaV>c0fO(pB`^%B|28sxj(B{JY3JtCP_%nIFcR~dCts$74wb!tL&Gqmjp#Y_2d zS;d!s(Sh7j7g%$jrq_j?QBV8XBaIbIZx?=CLY`7(T(o84L@V+R$arz8U?bTKb-cx% zc&{bAZRD37U*1oclE}&xYm0JsRIJD?m0MHEw{$mDAJ$apAl*{EuBlKx-cqFvDh~3u zRJ5k@Ywh!z3SW{PdII059eN_(`P6P$n9{v}=gVSSQDwXG>4t?f-Vof3=vP#n6yNLq zvO(onJ`HOsax1;oesGpIz@aR+(w;W7_kJ}$pV!-HJE4+K%CthnsUQ*QKB4mXF2h?T zbod^~$j7t}Um$p-QytwK&q!}G<>&xaQ)7&*~lDnjz5n_IvvE~^M!1_a{^E0hwE z0I!|i!wch2Z{^0q$P@nGjQ>kh`G@I$hu9tolQ<6@0y8#~R}V9PFHB`_XLM*WATu;D zF*ZIv3UhRFWnpa!c%1CLd3+Vs**JdAnSJiey>s_9$s{+qS#BVZ1qennvZ~0wNRUV( z2@tkG2ndP_E`Ye9t!u6OzLWp~17e|;+FGU1*5%b!r7bGjqP&(;6?5CUOIpAnt4xbU2z&A=i>;?d~!i+bIY-tZgL?s|6;hu zkOfe2P3k%cj&FeD!Uan@SKsY@s|SvshU2}9+vheL6Uz}cvW}^@J>(NkjFGJgxC`BfKE!t_JCOxXSqCkDj5^T$TqBRu^V8oz5Ax9!&<7rkM+fnK zl7zdrqEB!LujNLt!r4K->7-~cd(EVr+uEAqTzA!sIK79}c;2EpouJ@tsXczo? z(97t5J*){Q(vPQ4pfD;&7s7bD(Qoj6F4em}RZkHOSSm%eaHbvo6#Wt%#&P^J(k^I1 zrCR3u*&=-wSvD04MP-qLD+~WBhY}{An>KbtLVC8A+fYp&z54VI4+l;uZKh z{5Jjz8A;}lACW(EKjfd`eh>^iS-;!|-H$DPDlr;*IzYd>=lD594Fx zd@_wJB42O|xaHi-{0R6>dIx_2ebz~g^?4wvAtT5{ z_+3U?$#SxV+)290+vFQg;vAfd^K)g~Sgw(4Px52=Is8igLH-l|6JfUSy6};xic7=|VvqRW(h%u9X_7QqYLsr5c1dr_O+cML zM?2Ar{dpcA;ns7bxt-`XQpJbKZ^&B~Rl`Ha6_$!+ zq94CT+W5`Hi@OowpMlZU;zG;`K6C?a54h){_lL+&xbgf6VG3RV z9B?(-fR?A%qcy@z{&#pj;_wVq#2*3aww9~p^I?B2NR`G;lDp5IU%++|4x!KB z_Y*icPk0G!=HEe6QGNQB^jkpPB_Lt%LzknA&`}uW=kSCJxc#VqDm8&@OON4}!FWGJ zlhaS7^RS8*q!*)!XfJv~5>T_0ggNfQzk~5!g<8p!bSKxEY6Cpn4p^`NTPs2S++>ZM zKJ9#~{=9SR&KX`iY-mk&RpsD86$1y9C(BApiW5bJ@q&CaFE=L^jf6wNfZyl!xDA(6 z*Bq)MOQOJY1f%kQ(eW`&W>=!Ai%-NaxL^Q1jyFR^bH9qFE)%N8oZY6&Y+}vKvs+rw za^81ZT6RmTuO&9jI#f5H+#DS@yAF~2eN!SM-Sgd zkgP+ex+3vWqr1ZKQM3nLT+!&}maa*YXO12f&ChQbP~L?{&W&H*h2kT+TuIghjbxAM z5=VAP>@j8=9SyqK+*ZDS^DR9Fy1Xf=wZvPRXV2{7nj7dd+(~%usIK6ZM?+^0;pU!^ zGdK1>8|5~Cj}En&^l>s;dQNq^(OC*|gahPV4FQT|$u&H#h$S z0R_a5|LyFW=1h%PWc&kBkkUyXsloXy+?7mrm6cHvkw(ItLLbj#$29}WSM`v1e3@aw z4$y~xCIN1n8-`Z^`t$SY1mE0aq08Z*Yu)6T_OXdBk8Vd+MY5rbG|@BrvuFI%>6vxe zGkupg#etH$nTqW1k`sOZTt>hY*6#_ z|HF%V(kJLuY!N$ z%;u)e%{}RLm&Z*bzIiu!g}kzP+32P$;q;{U+#Ky1b4vr@Y5^V&OhM3yZE?J5@-_=^ znmTLdZUgM6P19y>Czyf~%4Mss_JU1wzL2-NX^cAdrQ=pU6aBd5*mPd$t!8wLP163i5A0fEL2 z)?~gr-(3U+hDG;VliUBTC7{#D*92ol@cop0m0pxMyBgM>5~l5vVbECczV)*-r4`{=YfGWc3scS+=Q3An8I5$Xp>~M^jP65mBaEQ}`L6l#R3sZ?gA(zW3 z=k}yi(DM<~0r--&1>lsWXgWZD3FiZ1Px@H5%SEOGaH?C^8F0j^rh^qliWafaEL>K& zu8=D%2oZXxkdxjfq|#f2Ol-o;nubB9BYFpVE0nzz0N+>+dMhN-&cX3Np)(Bl%2Mgw zQDgj^r$fmJ#>u4p*H|~c7j|_<<0*O=J{md>J5*n1z^}fpx6V^rkxce~!a;3?Cs>Q! z!Pj8Jra!PE;aYb=xYh%k z(~8v^1wPpFVDr~zZqiWY&aVuB>LN;kU?Av?a|20nA|7``^^hSo)roliBV_Y|H?I8k z@5h%+zc_vJmFY__8IV7J?oarU8}FWQ&ts`U!k&r0TJyl$IYosNR;HHY!8hD8%pvuz zp-$ZC%D4!R7=yii%iwji5k&) zh0TY;=0jogq5MD22P0z!C`w-qn~@XQ8PU$U^!sLHf^n*G{K=z#bS`?cGf`*s)(sv6 z81m<5wnzEGZy)56-@e7&AnbV|RsVcSe*tsY9VU64`8z!Sk@P1*0bsy~HA_vnX7V%T*JM6GCpO^qS6B1r z$Yc16}ES`TarQnI#oBR?>?@#+KHM7p@{MqK*H zi<-MyHlYAH*)VueGKrHQ7rSL^Of8Ii|E@8$vQ=q=m9-K`NFb^Yc7*|~v_V=MXQ1VX z*Gf)*A8cNFd~%l;z#JRQ0nksuueS9GReVN3CmW!wa#vxP{y26&@Jo*D`Q^7MVbAIH z{I%bV;n$sB2bj7SA<+f%Y;ZwKlgk`Hw1k!0h&>(|RyBq)*{Qo3{eRv~0hpT4tr7}p z_EfnvPC=N+io=Oyg{TgZPJ+XrhzE-KW*1w3@C`uq$K9Ff`6@d--`djy3sCYPD?kA3 z-)|f~yx$FLQ8H;u0u;58Y~@KNoj#G394mQN3apeV6~!5kYOIAw=a@KY2{cC;iIC9)!d!G1+2& z4%4xmtT@Tm+4{OVdoYdtrAJYFB+RPQDCad%61%TIGpNur0ioX$Z4@d*NvHC-Ph z$N%fjZFkmNkKr7M%I~Qi0m04IAnE+(EmKoUSAv0byg(hL0zzIF%=^bv$RbdeFAm&4i zqmDD!2;-ySw>SOXr{rNOxpIJX<=z#LAM)-ugRPrtbL>g?!|@p(m{c64W5IAmr^sfre*?DeR;

XaifFog`;B!ax`dpZ1Cfa;qm(BV+-dl46j_TbV86 zJX_M`Od}9q6y35&OK5x2KBjce*71|p`D}G(lP3k()Ub07kv*7Ar(nxWC%Zp`B?e=s z%dl6x?hIhlF_kt}!x`=oSXp5VGUm$*lqO>nx5ao(cvakQoG=`+(12%ukV(b@N0;$; z?eF^EoeHno}9wd~lVL z3pAW7a$*lzW+_P4KD9uJBYUs|VeGIxnu%H^ZptM75dR_1ZLzf#%P~pYFMX(STQscE zW5XpKlE}5vI*CX>aJ~J14hZ^_jmyKZ!N1UPn+T5s4c0}D*B=FRJ;*>$Ubabw~2MR)3VyYBYqW)`~v z`3X-n5m6GQIDt@U#KTt(PM}t>!}S4cqBliJv?!nt$Sp3yMM6NJtZFl9?f@k>H^5Py z@OTnvwh^|>t`XCfsN%T&i&_JU3Z1$`$dBdZMnUBV&0oYtg#}OtHdAy!#G)i|djv<0 z2hada6)m=Z_fD)(_m-eJT z>vlTH^itaGJbKU4NM*i8ugPabn=jMGfB~HFP$iKz_GGEm(f{1WoQh4An0nSarqgZ#QhK0Qtcg@OR8+^ zv^Y^%oJiDE4;fMgR%}p8B;p04-xmzMB#|1$OW`|vx)(pxVXv0~Kd`SDYEj8KJ6Ekdw4(D<%31rMSPz`f;Q}j6Y&K^lVHO|v@9aLpEI#gg z>^>$QFxY$qh-m+gVdqw}eO0x+A27(?mz3K3c#*x&$+h>P2-{m_x>0QkTZF9w*a9Hk zZ$}TKF2q+L3r#{Fq7z8)m{7R|ajZF0_E9K*BaZ%-6-S?E#nCCtu$Km=Up>md-A_5E zE{N@*#KJT$IQbcWgvW5P25JV z3(3K(T^P(#ZIE`OfTAauFcq9<(Y`56geofSuo7Z5l!%MjG8xIsF&O$l%eRKKb!K%W|n# z5^VMNg*TBCMRC9y7ja>q;qyg<(J0Rwyw4GIMEPfeyPU5&xnM99C1#H0p6Hzzv?4Qw znaU-`boU(Ztl*r`jL0R?n}hceBb>`|o?M6G&rX^@J86D8X^f`*2~)xnl|@W{DWaLLM9ZKza`d2vToj>5{nosY4_)Y2^@@u>V|M->E6My_L_3+ML;aNXT{WEn4SL5go z?D#?IBb)a5UZ8y)h0qBrx7EGKN5&cBe3u%R`gn(y3zh>4hM1dE_GD4&$)c3X+g+5N zJqej&*G*wrr8tz0jNx_C8bC)bN6ZL@f05AlI7R+b-=>za(f`$x7Vgh~4q1MfU@UL! z?+eMg(wHNu5!oc2O8~R^`OL|frDjrk=lI2UHhi9XEwu?>wfDisiwED3x=GmM^tA3; z@=~g|_jwNAa_#Kv{W_Jr^MHm|fliZyzSudJEX*O;)<;+mk6CjlWTHxaE?R~`RoUE-e+ULY*Wxyp01 z=YH-!=l!v#@#Exa_ghXc@}Y>~Ga@{73T!W_WsC6Wl3K%sFdy~iYFspzSB!+~B9t&O z#*w@r!+Vh7J*Z}cmyBgbq%uRT9M0_{&n$7C0wA+a!pg{`Xd|V6Oppy7I4IfRjTxUpY7R^N)_F-ucm1Jo1%4;_`F$RlV|qXZ|vK$;TTW`!gYfzc~Ff zy!3Y;;py9syguOJJ0DGbamP!kPdD$SSPdg2t)ejnvUWFerBk*sX)kO}n*@N_deXg_ z6I|a3#*38jN|{Z6B-*+zTkF{1c_w^0RuV09*-55=x+ueE09#W5ZS&1;>i&Sm=}n6{ zLP}A>UFu1MYS9pPt!GH+LNwNWp=WGpCc4Bu({oA4xL3Z{MKa1-m4PGSq`z9I)(AZ+Ic{`?_Ldv$t?eE)1Omji{)?cpj9LGPM)oqk0w>h$ z&@91%Mxf~7jz-kyE<5v=kE^`Q*st<>VCMy+3{!i54C4n=SN-zC!pJZc2S5AW#CUAL z$3IUkeJS;Nu@v;BUK93wTYt}8e=X!b=#8ZQ_V=5+xu1M9hHt#ZY#n?0F&Zuav23BqR2^n488&+laQK$H}6F8VePMSeQ_w{TG?27M(0)Xwd$OgNOAkeRTC(Zre7= zqyhHIDW(zjMG4r-nEdD?D$tK6>yP(;CXc#Vps(-R<+fQ^%S=LQ05QvlIxEofB5K7j z8)qo{)l?T#i7yqON0`wQ@cVs%U_8Nzk`tBzx(YxeuC8VG!mWG9uDGCP(R=f8)#y#v zuF2^NEj@hGre`M^O0ZyWEO_~W_Suz7+7>*T$hm&{nCEVsuzrHisYeQn)TIM|&TUv8 zT7L65tNEgVt52N1@!Vnf{gRkbGQQ%1rb{QDdj;jDO|WFtQR@du_$jb|)H6aq3XwHx?U@XEhs#lL5`IoD)g&fnF8#|!``rta=;{;!7)o%ZvI)9=x>eR6c$?)FIMGX;iH&vP`Tk|F$#u%6Gs7Sk@2jG(263(LG5P;#fn3vvco~uUnXW9R@IQ|v;)IG z$Al1_oAT-*)m@;uH659ldOP<<-3_$cCj$&cE}G-o2Tb7ZWh^aq{Qg$eWoq zqo2KD1MV!4lVzfRSQdz8fk+m}$vW3#Ss`Z_93?T}* zvw$~Cb4C{MWC3>;(6h#*oCTDg^nY37fuM`{qx@0jPr;8&;Vt2mNrJK&S3*%!;kbBi zO!UWK)`7m`k+7j2F2Y-i9xft9pprR@wz#pIr}W|u(PhJb%~n53HEtiJ8aGuHf|PRH zgi?-Mqg3o>>z+GnP`R`IeRmITv~ojoza$hU#6m?|qBzPP5bb+FlszCyqknGtfG8u) zs7fCIwJDoTq8fcvG`qw`=@Uv2sfx4P#~Clg85hKh@L`0vpobC3L-lAP$aH#_9L54H zc!-RwH3(&YAj1gXGGWCu2Or}ICaDlxCQ-Pss0Xj!kx!*oazfu?$mABwxj1U{t7OY( zzxA}2X|KyWfFJ6BBkIN*#|>%>Q9+ks4JT;h37_VUVvp|6u*7<5VGi{*}i)9r2FsaeHT{5QJ`NI11ISyjQ8#GhbSJrRCA<&i;_btg~IHFM^eMu zSn&dRhS(s_7u#gH+8FK`9;gY8HpY3z1xANv3$v9eMx&=OFeS7^SfaETOFT;gEuky0 zUl9d={ZbC>8ue0bG1n@zs*5!>7~>^3kZb>3ij&#wimxm$oo`=ZlzGmgObAnF3l(Mn zG4GjVWNJ@jT`(uIE|?SDbcQo-8V2mQoP|ZzgCvY3Loy|fI&5sevNZTZpjp~t3DwX5 zb~0jf+Lj+9KW8D*oYd#wVdSMT@`^F?ax(INvZ;vi3<@y1vfy!)JPFd+s?P3aN7m5b zNcGUn>Ubu^Ly5{*o@_h?=JVN-HtX-9F-Dq0rA!s3DwhkFD?Dy&VDVtDF%&2S*-m8S zWqMT&qmWUL-}H+=;J{UXz4^n`@!i`uZrHx##*N#F7Z=~QD)pz{gMYma=VJYh*WdVm z?Jr*cHFS1N`WW{i=t*vrgZEi$RZv%o^lE*SF4XvHVwaF<>J;D9*nH9=v?_CbO|kuX zZwhaD-w%J}{mA!4@UP*Ia*pJs^8$H!$w*zGE;25%EN@GmG>{bP0|UcJjXsWy*2nlR zj9sG6(C6zPiJt_%!6%&t_H#~$;X=`Wm_u?S)ejndC@T&r@FJ5mRdo6L@$ z4c+$>eL$T}Os02w7@>IB)Or}9cnT#JjG}prBc|_@4oM$MX^GF1>ZOT5bh(UwVwtWk zLsbAg>U3r$LKQ}DvhHXmi-&G*s#Z~zoY6{t z4OPZL8}~370-6-=M;Y9()&tkRwQ}K`*Eii=v7^^~e&wnsp1x}JBO4yP<@93@VQ%x} z^NI7DF~sx6uYdOHdv6?|TJ$)7kQuokGyFgu4_d)I6!VkmT%*vaOn0<$i-dNi)gk+- zA(cVZQELi?a$>YB_Pi^6<2x1M2YZHx2glC$jE|fjo9vk#o)T;JEQvJ7R*S3sr^u<0 z0g9bV4+bX%XrdGsh`F{H4;#cV_-IU(&>r$E!eDOEX=H9KHdO}3_%3gMjCTZWwm;h) zZ@VFb7Obs*WL`tvcIJu{_>4`lPH&|YmsNM^SdZk<5Jgd#wtAPeMY0h4m+ zwakJl@D&VKE>cDn&M0UtSgYKo+)((W_qp;{I9&-wLcu}f%HIwOQ8JwnqY|s3+45{< zwmRD}Tbr#flou)s)rF3Q+CsfM(OvAKp}4}*A%(Nl21iSxrKB_7S-7t72kHabohA2_ z-!SyWb|ik02$W>Cu!1ZQ&jN*6ATwSuI|MO1C^0)sk*b*=Sf1S4S#oia zrt%Rp;pZI#b0XBcQ4lU?0xDb|o*14J-Wom>7G2@IaC`W}FrOE`Jxszc!(!tHCSK4QA7QoVD9Nw!Czq z z*}D*hZ1s+Rt+u9(e8Px?jfhc89_^dSsL(8QF&)9h`tCAmV;41v60{30mfq!Jve(6u zU0ekpB1|<(g2IC{+}OB$e3m0A?> znC@)HNycviN}*LeAU7`f$`fwGV|WctEYQs;Qc9$MC>91lk?VuQd}ln03gWsZm#R@* zQmm+Al8>T1BZsPyN$MuG3#LYvCD*TCkNPbX)WU8&Q$ZVfhX(9rpg2)HkknMuINf(b z((t2k59ZCO-|o8Us;{ zft_*1%)5fjiDOxJ(lI;0?l$eynLMu#vJRsUc2FRjRq3JX3R^mCPo|j31Aenx(}{gI z2Zh|GW}n#K?X*2O`>hHJxh;0Ax;czAQ4+D}Qjw}_B748WjR~*vZZ%^Ie#IEe4fK0| z@SySbLE}x9V3`RzfP!&`n4(s|<-=ussY)(#U+TWi&C&6&q{ky!uh5aKI&i{L^75;V zSWa=ZLXTd|E3D>4P4S9RCF~J+#ETBa;gmfF@^U^YCPy7PpnDfdWpdJ4jcTOf@;S~? z+*r|)#>?XzBVA+N7kMsqP4O&}TIBhEo;Bi?Qm4FI+~eBi`GEjbEO8Zk zDtyDxP|p?e2Kip@9_=aoGdF5ZLOvLqKtt|F(W2qCA$=~ycQW#VmXC|%@rQiRJWfOYJD^NOm& zolukRY5Wv*wtKOAt$VXub*nswFr5y2V!j*SsYsrzuyaq0qx5IX5%?Fid>o7WNP?oO zGLW-sxT#`1Zij$8APz6I=BX~H`E$1AF(}9XP4VKDe9) zgNp265F%Le@UqLTId#@OkFIHd^j!!-u;_8osITffW$3s`U#73qIlTuzWvS*w6}PL` zszmJ}(=BD98@Ic!b<_0u>6XI~aFejiPVdA|?Zl_Nr{*yU8y zxUTA{ERXr{Hfa!M75P)gbycwlk(@rVO)~9TPbPVfzQA&L7pyOEZ;%9sw@ZWQ9k-)l zWRLx%zMJ=57i8DD(?@owCU2r)nIKcj%la)9;bBJHI^yvbr$+B`Fi`LqVLk8)+GLmm}-8A#rl$BzV9Svf@H%w2UjseTTTm)jqKVZ8(q&;LI3gIE! z=Jj`81J+L-HI8R;XpWw&WA;`rSX;n^RaM4o{a$w}3feZ8J6==e9_p_0$L%*Mh!MHC z?()UuH{7se=T2{bvLyGBhm7-DA0=~d!P4T?t+(|4V0?K5Fg*$M>f@kSNATZwqabx- zGc+<|Lt&-MHauAAmd`8^R<-k57j`)?O`BSVIK+Fvm=1wKgq?m#W+>5Yo0B2K_MWi7 z&cQcdwe#E#G*;4W@SvzQR?%=?Y@~N&aH@A|u*usL{1N$o5qH1-xbb*Ill8E=khF0N zg_RoRe~_cfE_J7-1+)#?UkK+cnB!`9UF+gpm}<9)K`fW63A#nyc1O?&q#&2efzI?X z239!*?U7Lv*DG`t@X?cQ0PeJfw%(Q&~he6Ove@rrC2N)GlT_n zEGBv_I_>y>0y^0^Lhpz#jQJV0`I(*OXEf(8JS1Uizmbf6#(b0#bK7SuIoqlm9IZak zMtO3q;oTSQQ2SYMwh3?u4qwy&GW9fsqad|_AeX~Ye#$wOv zV^mw?>bB*4@ssyb|LpkmrWgL0w>5n2tWD28e#63l+whIS7Z2eatUiy)`mK*d7cKtz z@85pqx=fVh7<e?$WZWp1qYaWCBPyk3X7E}ELNiQ#Yz+wn@}ROox)Q8DS{Oi zD`7c*p3DiXuvm%0-m0QmaTC}}S*0{S5;zec%K{Gvx&r9{A0R$9c|JBBJ~ny2BKwUV z4e;X)~zY{14^b3>_W@1e6e>aSk35!tuzn{|2&SbLcV2<6jXJR5w(J2)HYG~#>XG%okJekc-7R~uI_ya zMz%IJnKTK%M+Q3AQj1*}86HVCj2>LI13l!FVQ;ymhn$xo&frXr^n-=<;a$HOKWp#$HO1$~p?|5#d+}G;`T2Xjrw=!O zY`*)Yms5Eunum2Gs0XitjB=w7tnn3IZ16bFSMwwJsr)>?lNS}YtjLP)bt^jJWbBC1 zkOWedk}Wcp3rsKek^(oIGdG(>H=9NGzn{Oa`dLYs5+yp9%Y%v?^>z zGYk5#QnOpwu4h*2tkkq@ILTshYK@^UBlvSvpNn&Fv9QY{Iz+GKahVQFGhNJodUu5@ zl9Bf#p@WgIL3eD#Oo@-~aK)(5{lHoht1a=(aBWpNOSfFaWtI%8HfSMfiYK6lJjIS; ztymwT4bf|y_qiPLtQS-0L@iJ zT-fLJ7I{=39Jn-4O^Y0=&*4zLG=0S((i@~i4(G&-u%Qw&>{sSVO5YTP6(tq~1c?5Lm%5~=TwnrlIS835$<(l