flow testing

unit tests are passing

ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow

@@ -18,3 +18,14 @@ param set-default LPE_FAKE_ORIGIN 1
 
 param set-default MPC_ALT_MODE 2
 
+param set-default MPC_ALT_MODE 2
+
+# Multi-EKF (off by default)
+param set-default EKF2_MULTI_IMU 0
+param set-default SENS_IMU_MODE 1
+param set-default EKF2_MULTI_MAG 0
+param set-default SENS_MAG_MODE 1
+
+# log ekf2 replay data
+param set-default SDLOG_PROFILE 3
+param set-default SDLOG_MODE 2

boards/px4/sitl/sitl.cmake

@@ -13,7 +13,7 @@ if(REPLAY_FILE)
 
 	message(STATUS "Building without lockstep for replay")
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
-elif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
+elseif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
 	set(ENABLE_LOCKSTEP_SCHEDULER no)
 else()
 	set(ENABLE_LOCKSTEP_SCHEDULER yes)

msg/estimator_states.msg

@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[24] states		# Internal filter states
+float32[25] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[24] covariances	# Diagonal Elements of Covariance Matrix
+float32[25] covariances	# Diagonal Elements of Covariance Matrix

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -116,14 +116,14 @@ void Ekf::fuseAirspeed()
 	_fault_status.flags.bad_airspeed = false;
 
 	// Observation Jacobians
-	SparseVector24f<4,5,6,22,23> Hfusion;
+	SparseVector25f<4,5,6,22,23> Hfusion;
 	Hfusion.at<4>() = HK4;
 	Hfusion.at<5>() = HK5;
 	Hfusion.at<6>() = HK3*vd;
 	Hfusion.at<22>() = -HK4;
 	Hfusion.at<23>() = -HK5;
 
-	Vector24f Kfusion; // Kalman gain vector
+	Vector25f Kfusion; // Kalman gain vector
 
 	if (!update_wind_only) {
 		// we have no other source of aiding, so use airspeed measurements to correct states

src/modules/ekf2/EKF/common.h

@@ -386,14 +386,15 @@ struct parameters {
 };
 
 struct stateSample {
-	Quatf  quat_nominal;	///< quaternion defining the rotation from body to earth frame
-	Vector3f    vel;	///< NED velocity in earth frame in m/s
-	Vector3f    pos;	///< NED position in earth frame in m
+	Quatf  quat_nominal;		///< quaternion defining the rotation from body to earth frame
+	Vector3f    vel;		///< NED velocity in earth frame in m/s
+	Vector3f    pos;		///< NED position in earth frame in m
 	Vector3f    delta_ang_bias;	///< delta angle bias estimate in rad
 	Vector3f    delta_vel_bias;	///< delta velocity bias estimate in m/s
-	Vector3f    mag_I;	///< NED earth magnetic field in gauss
-	Vector3f    mag_B;	///< magnetometer bias estimate in body frame in gauss
-	Vector2f    wind_vel;	///< horizontal wind velocity in earth frame in m/s
+	Vector3f    mag_I;		///< NED earth magnetic field in gauss
+	Vector3f    mag_B;		///< magnetometer bias estimate in body frame in gauss
+	Vector2f    wind_vel;		///< horizontal wind velocity in earth frame in m/s
+	float       posd_terrain;	///< vertical position of the terrain surface in m
 };
 
 union fault_status_u {

src/modules/ekf2/EKF/control.cpp

@@ -431,11 +431,14 @@ void Ekf::controlOpticalFlowFusion()
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air
 						     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
 								     || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
-		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
+		/* const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); */
+		const bool flight_condition_not_ok = !_control_status.flags.in_air;
 
 		_inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
 				    || !_control_status.flags.tilt_align;
 
+		controlHaglFlowFusion();
+
 		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
 			if (!(_params.fusion_mode & MASK_USE_OF)
@@ -466,13 +469,14 @@ void Ekf::controlOpticalFlowFusion()
 			}
 		}
 
-		if (_control_status.flags.opt_flow) {
+		if (_control_status.flags.opt_flow || _hagl_sensor_status.flags.flow) {
 			// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
 			if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
-				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
-					fuseOptFlow();
+				fuseOptFlow();
+
+				if (_control_status.flags.opt_flow) {
 					_last_known_posNE = _state.pos.xy();
 				}
 
@@ -901,18 +905,19 @@ void Ekf::controlHeightFusion()
 			fuseGpsHgt();
 		}
 
-	} else if (_control_status.flags.rng_hgt) {
-
-		if (_range_sensor.isDataHealthy()) {
-			fuseRngHgt();
-		}
-
 	} else if (_control_status.flags.ev_hgt) {
 
 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
 			fuseEvHgt();
 		}
 	}
+
+	// Fuse range finder data if available
+	controlHaglFusion();
+	if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) && _range_sensor.isDataHealthy()) {
+		fuseHaglAllStates();
+	}
+	updateTerrainValidity();
 }
 
 void Ekf::checkRangeAidSuitability()
@@ -922,7 +927,7 @@ void Ekf::checkRangeAidSuitability()
 	    && isTerrainEstimateValid()) {
 		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
 		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-		const float range_hagl = _terrain_vpos - _state.pos(2);
+		const float range_hagl = _state.posd_terrain - _state.pos(2);
 		const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
 		const bool is_in_range = range_hagl < range_hagl_max;
 

src/modules/ekf2/EKF/covariance.cpp

@@ -202,7 +202,7 @@ void Ekf::predictCovariance()
 	}
 
 	// compute noise variance for stationary processes
-	Vector24f process_noise;
+	Vector25f process_noise;
 
 	// Construct the process noise variance diagonal for those states with a stationary process model
 	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
@@ -217,6 +217,9 @@ void Ekf::predictCovariance()
 	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
 	// wind velocity states
 	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
+	// terrain vertical position state
+	process_noise.slice<1, 1>(24, 0) = sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise) + sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
+			* (sq(_state.vel(0)) + sq(_state.vel(1)));
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
@@ -482,7 +485,7 @@ void Ekf::predictCovariance()
 
 
 	// covariance update
-	SquareMatrix24f nextP;
+	SquareMatrix25f nextP;
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 
@@ -862,6 +865,37 @@ void Ekf::predictCovariance()
 
 	}
 
+	if (_params.terrain_fusion_mode > 0) {
+		nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13;
+		nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12;
+		nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11;
+		nextP(3,24) = P(0,24)*PS13 + P(1,24)*PS12 + P(10,24)*PS7 - P(11,24)*PS6 - P(12,24)*PS34 - P(2,24)*PS11 + P(3,24);
+		nextP(4,24) = P(0,24)*PS174 + P(1,24)*PS173 + P(13,24)*PS43 + P(14,24)*PS172 - P(15,24)*PS171 + P(2,24)*PS175 - P(3,24)*PS176 + P(4,24);
+		nextP(5,24) = -P(0,24)*PS202 - P(1,24)*PS204 - P(13,24)*PS193 + P(14,24)*PS75 + P(15,24)*PS190 + P(2,24)*PS201 + P(3,24)*PS203 + P(5,24);
+		nextP(6,24) = P(0,24)*PS216 + P(1,24)*PS217 + P(13,24)*PS199 - P(14,24)*PS197 + P(15,24)*PS87 - P(2,24)*PS214 + P(3,24)*PS215 + P(6,24);
+		nextP(7,24) = P(4,24)*dt + P(7,24);
+		nextP(8,24) = P(5,24)*dt + P(8,24);
+		nextP(9,24) = P(6,24)*dt + P(9,24);
+		nextP(10,24) = P(10,24);
+		nextP(11,24) = P(11,24);
+		nextP(12,24) = P(12,24);
+		nextP(13,24) = P(13,24);
+		nextP(14,24) = P(14,24);
+		nextP(15,24) = P(15,24);
+		nextP(16,24) = P(16,24);
+		nextP(17,24) = P(17,24);
+		nextP(18,24) = P(18,24);
+		nextP(19,24) = P(19,24);
+		nextP(20,24) = P(20,24);
+		nextP(21,24) = P(21,24);
+		nextP(22,24) = P(22,24);
+		nextP(23,24) = P(23,24);
+		nextP(24,24) = P(24,24);
+
+		// add process noise that is not from the IMU
+		nextP(24, 24) += process_noise(24);
+	}
+
 	// stop position covariance growth if our total position variance reaches 100m
 	// this can happen if we lose gps for some time
 	if ((P(7, 7) + P(8, 8)) > 1e4f) {
@@ -898,7 +932,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	// and set corresponding entries in Q to zero when states exceed 50% of the limit
 	// Covariance diagonal limits. Use same values for states which
 	// belong to the same group (e.g. vel_x, vel_y, vel_z)
-	float P_lim[8] = {};
+	float P_lim[9] = {};
 	P_lim[0] = 1.0f;		// quaternion max var
 	P_lim[1] = 1e6f;		// velocity max var
 	P_lim[2] = 1e6f;		// positiion max var
@@ -907,6 +941,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	P_lim[5] = 1.0f;		// earth mag field max var
 	P_lim[6] = 1.0f;		// body mag field max var
 	P_lim[7] = 1e6f;		// wind max var
+	P_lim[8] = 1E4f;		// terrain vertical position max variance
 
 	for (int i = 0; i <= 3; i++) {
 		// quaternion states
@@ -1056,11 +1091,20 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 			P.makeRowColSymmetric<2>(22);
 		}
 	}
+
+	// terrain vertical position
+	// constrain variances
+	P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]);
+
+	// force symmetry
+	if (force_symmetry) {
+		P.makeRowColSymmetric<1>(24);
+	}
 }
 
 // if the covariance correction will result in a negative variance, then
 // the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
+bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP)
 {
 	bool healthy = true;
 

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -46,8 +46,8 @@
 
 void Ekf::fuseDrag(const dragSample &drag_sample)
 {
-	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
-	Vector24f Kfusion; // Kalman gain vector
+	SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
+	Vector25f Kfusion; // Kalman gain vector
 
 	const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
 	const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)

src/modules/ekf2/EKF/ekf.cpp

@@ -108,9 +108,6 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes();
 
-		// run a separate filter for terrain estimation
-		runTerrainEstimator();
-
 		updated = true;
 
 		// run EKF-GSF yaw estimator

src/modules/ekf2/EKF/ekf.h

@@ -51,15 +51,15 @@
 class Ekf final : public EstimatorInterface
 {
 public:
-	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
+	static constexpr uint8_t _k_num_states{25};		///< number of EKF states
 
-	typedef matrix::Vector<float, _k_num_states> Vector24f;
-	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
+	typedef matrix::Vector<float, _k_num_states> Vector25f;
+	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix25f;
 	typedef matrix::SquareMatrix<float, 2> Matrix2f;
 	typedef matrix::Vector<float, 4> Vector4f;
 	template<int ... Idxs>
 
-	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+	using SparseVector25f = matrix::SparseVectorf<25, Idxs...>;
 
 	Ekf() = default;
 	virtual ~Ekf() = default;
@@ -125,7 +125,7 @@ public:
 	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
 
 	// get the state vector at the delayed time horizon
-	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
+	matrix::Vector<float, 25> getStateAtFusionHorizonAsVector() const;
 
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
@@ -137,10 +137,10 @@ public:
 	float getTrueAirspeed() const;
 
 	// get the full covariance matrix
-	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
+	const matrix::SquareMatrix<float, 25> &covariances() const { return P; }
 
 	// get the diagonal elements of the covariance matrix
-	matrix::Vector<float, 24> covariances_diagonal() const { return P.diag(); }
+	matrix::Vector<float, 25> covariances_diagonal() const { return P.diag(); }
 
 	// get the orientation (quaterion) covariances
 	matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }
@@ -228,13 +228,13 @@ public:
 	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
 
 	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
+	float getTerrainVertPos() const { return _state.posd_terrain; };
 
 	// get the number of times the vertical terrain position has been reset
 	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
 
 	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
+	float get_terrain_var() const { return P(24,24); }
 
 	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
 	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
@@ -418,7 +418,7 @@ private:
 	bool _non_mag_yaw_aiding_running_prev{false};  ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
 	bool _is_yaw_fusion_inhibited{false};		///< true when yaw sensor use is being inhibited
 
-	SquareMatrix24f P{};	///< state covariance matrix
+	SquareMatrix25f P{};	///< state covariance matrix
 
 	Vector3f _delta_vel_bias_var_accum{};		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
@@ -534,10 +534,9 @@ private:
 	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2
 
 	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
+	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _state.posd_terrain has been reset
+	uint64_t _time_last_hagl_fuse{0};	///< last system time that a range sample was fused by the terrain estimator
+	uint64_t _time_last_fake_hagl_fuse{0};	///< last system time that a fake range sample was fused by the terrain estimator
 	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 
@@ -627,6 +626,7 @@ private:
 	void fuseGpsHgt();
 	void fuseRngHgt();
 	void fuseEvHgt();
+	void fuseHaglAllStates();
 
 	// fuse single velocity and position measurement
 	void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
@@ -689,12 +689,10 @@ private:
 	// initialise the terrain vertical position estimator
 	void initHagl();
 
-	void runTerrainEstimator();
-	void predictHagl();
+	void controlHaglFusion();
 
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void controlHaglRngFusion();
-	void fuseHaglRng();
 	void startHaglRngFusion();
 	void resetHaglRngIfNeeded();
 	void resetHaglRng();
@@ -706,7 +704,6 @@ private:
 	void startHaglFlowFusion();
 	void resetHaglFlow();
 	void stopHaglFlowFusion();
-	void fuseFlowForTerrain();
 
 	void controlHaglFakeFusion();
 	void resetHaglFake();
@@ -739,9 +736,9 @@ private:
 	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
 	// that is optimized by exploring the sparsity in H
 	template <size_t ...Idxs>
-	SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
+	SquareMatrix25f computeKHP(const Vector25f &K, const SparseVector25f<Idxs...> &H) const
 	{
-		SquareMatrix24f KHP;
+		SquareMatrix25f KHP;
 		constexpr size_t non_zeros = sizeof...(Idxs);
 		float KH[non_zeros];
 
@@ -768,7 +765,7 @@ private:
 	// measurement update with a single measurement
 	// returns true if fusion is performed
 	template <size_t ...Idxs>
-	bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
+	bool measurementUpdate(Vector25f &K, const SparseVector25f<Idxs...> &H, float innovation)
 	{
 		for (unsigned i = 0; i < 3; i++) {
 			if (_accel_bias_inhibit[i]) {
@@ -779,7 +776,7 @@ private:
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
-		const SquareMatrix24f KHP = computeKHP(K, H);
+		const SquareMatrix25f KHP = computeKHP(K, H);
 
 		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
 
@@ -798,7 +795,7 @@ private:
 
 	// if the covariance correction will result in a negative variance, then
 	// the covariance matrix is unhealthy and must be corrected
-	bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP);
+	bool checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP);
 
 	// limit the diagonal of the covariance matrix
 	// force symmetry when the argument is true
@@ -809,7 +806,7 @@ private:
 
 	// generic function which will perform a fusion step given a kalman gain K
 	// and a scalar innovation value
-	void fuse(const Vector24f &K, float innovation);
+	void fuse(const Vector25f &K, float innovation);
 
 	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
 
@@ -847,7 +844,7 @@ private:
 	bool otherHeadingSourcesHaveStopped();
 
 	void checkHaglYawResetReq();
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.posd_terrain : _last_on_ground_posD; }
 
 	void runOnGroundYawReset();
 	bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -74,7 +74,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
 	_information_events.flags.reset_vel_to_flow = true;
 	ECL_INFO("reset velocity to flow");
 	// constrain height above ground to be above minimum possible
-	const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+	const float heightAboveGndEst = fmaxf((_state.posd_terrain - _state.pos(2)), _params.rng_gnd_clearance);
 
 	// calculate absolute distance from focal point to centre of frame assuming a flat earth
 	const float range = heightAboveGndEst / _range_sensor.getCosTilt();
@@ -289,7 +289,7 @@ void Ekf::resetHeightToRng()
 
 	// update the state and associated variance
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
+	resetVerticalPositionTo(-dist_bottom + _state.posd_terrain);
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
@@ -673,9 +673,9 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 }
 
 // get the state vector at the delayed time horizon
-matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
+matrix::Vector<float, 25> Ekf::getStateAtFusionHorizonAsVector() const
 {
-	matrix::Vector<float, 24> state;
+	matrix::Vector<float, 25> state;
 	state.slice<4, 1>(0, 0) = _state.quat_nominal;
 	state.slice<3, 1>(4, 0) = _state.vel;
 	state.slice<3, 1>(7, 0) = _state.pos;
@@ -684,6 +684,7 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 	state.slice<3, 1>(16, 0) = _state.mag_I;
 	state.slice<3, 1>(19, 0) = _state.mag_B;
 	state.slice<2, 1>(22, 0) = _state.wind_vel;
+	state.slice<1, 1>(24, 0) = _state.posd_terrain;
 	return state;
 }
 
@@ -780,7 +781,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
+			vel_err_conservative = math::max((_state.posd_terrain - _state.pos(2)), gndclearance) * _flow_innov.norm();
 		}
 
 		if (_control_status.flags.gps) {
@@ -817,7 +818,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 
 	// Calculate optical flow limits
 	// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
-	const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
+	const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_state.posd_terrain - _state.pos(2)), 0.0f);
 	const float flow_hagl_min = _flow_min_distance;
 	const float flow_hagl_max = _flow_max_distance;
 
@@ -986,7 +987,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	*status = soln_status.value;
 }
 
-void Ekf::fuse(const Vector24f &K, float innovation)
+void Ekf::fuse(const Vector25f &K, float innovation)
 {
 	_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
 	_state.quat_nominal.normalize();
@@ -997,6 +998,7 @@ void Ekf::fuse(const Vector24f &K, float innovation)
 	_state.mag_I -= K.slice<3, 1>(16, 0) * innovation;
 	_state.mag_B -= K.slice<3, 1>(19, 0) * innovation;
 	_state.wind_vel -= K.slice<2, 1>(22, 0) * innovation;
+	_state.posd_terrain -= K(24) * innovation;
 }
 
 void Ekf::uncorrelateQuatFromOtherStates()
@@ -1287,7 +1289,7 @@ void Ekf::startRngHgtFusion()
 
 		// Range finder is the primary height source, the ground is now the datum used
 		// to compute the local vertical position
-		_hgt_sensor_offset = 0.f;
+		resetHaglFake();
 
 		if (!_control_status_prev.flags.ev_hgt) {
 			// EV and range finders are using the same height datum
@@ -1300,10 +1302,6 @@ void Ekf::startRngAidHgtFusion()
 {
 	if (!_control_status.flags.rng_hgt) {
 		setControlRangeHeight();
-
-		// calculate height sensor offset such that current
-		// measurement matches our current height estimate
-		_hgt_sensor_offset = _terrain_vpos;
 	}
 }
 

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -170,7 +170,7 @@ void Ekf::fuseGpsYaw()
 
 	// calculate observation jacobian
 	// Observation jacobian and Kalman gain vectors
-	SparseVector24f<0,1,2,3> Hfusion;
+	SparseVector25f<0,1,2,3> Hfusion;
 	Hfusion.at<0>() = -HK16*HK18;
 	Hfusion.at<1>() = -HK18*HK24;
 	Hfusion.at<2>() = -HK18*HK25;
@@ -178,7 +178,7 @@ void Ekf::fuseGpsYaw()
 
 	// calculate the Kalman gains
 	// only calculate gains for states we are using
-	Vector24f Kfusion;
+	Vector25f Kfusion;
 	Kfusion(0) = HK27*HK32;
 	Kfusion(1) = HK28*HK32;
 	Kfusion(2) = HK30*HK32;

src/modules/ekf2/EKF/height_fusion.cpp

@@ -90,7 +90,7 @@ void Ekf::fuseRngHgt()
 {
 	// use range finder with tilt correction
 	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-					  _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+					  _params.rng_gnd_clearance)) - _state.posd_terrain;
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -196,8 +196,8 @@ void Ekf::fuseMag(const Vector3f &mag)
 	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
 
 	// Observation jacobian and Kalman gain vectors
-	SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
-	Vector24f Kfusion;
+	SparseVector25f<0,1,2,3,16,17,18,19,20,21> Hfusion;
+	Vector25f Kfusion;
 
 	// update the states and covariance using sequential fusion of the magnetometer components
 	for (uint8_t index = 0; index <= 2; index++) {
@@ -623,7 +623,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 
 	// calculate the Kalman gains
 	// only calculate gains for states we are using
-	Vector24f Kfusion;
+	Vector25f Kfusion;
 
 	for (uint8_t row = 0; row <= 15; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
@@ -679,7 +679,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 	// apply covariance correction via P_new = (I -K*H)*P
 	// first calculate expression for KHP
 	// then calculate P - KHP
-	SquareMatrix24f KHP;
+	SquareMatrix25f KHP;
 	float KH[4];
 
 	for (unsigned row = 0; row < _k_num_states; row++) {
@@ -809,12 +809,12 @@ void Ekf::fuseDeclination(float decl_sigma)
 	// Calculate the observation Jacobian
 	// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
 	// Note Hfusion indices do not match state indices
-	SparseVector24f<16,17> Hfusion;
+	SparseVector25f<16,17> Hfusion;
 	Hfusion.at<16>() = -HK0*HK2*magE;
 	Hfusion.at<17>() = HK4;
 
 	// Calculate the Kalman gains
-	Vector24f Kfusion;
+	Vector25f Kfusion;
 
 	for (unsigned row = 0; row <= 15; row++) {
 		Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -62,6 +62,10 @@ void Ekf::fuseOptFlow()
 	const float ve = _state.vel(1);
 	const float vd = _state.vel(2);
 
+	// get latest vehicle and terrain vertical position
+	const float pd = _state.pos(2);
+	const float ptd = _state.posd_terrain;
+
 	// calculate the optical flow observation variance
 	const float R_LOS = calcOptFlowMeasVar();
 
@@ -82,7 +86,7 @@ void Ekf::fuseOptFlow()
 	const Vector3f vel_body = earth_to_body * vel_rel_earth;
 
 	// height above ground of the IMU
-	float heightAboveGndEst = _terrain_vpos - _state.pos(2);
+	float heightAboveGndEst = _state.posd_terrain - _state.pos(2);
 
 	// calculate the sensor position relative to the IMU in earth frame
 	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
@@ -120,119 +124,209 @@ void Ekf::fuseOptFlow()
 	const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
 	const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
 	const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
-	const float HK3 = HK0*vd + HK1*ve + HK2*vn;
-	const float HK4 = 1.0F/range;
-	const float HK5 = 2*HK4;
-	const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
-	const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
-	const float HK8 = HK0*vn - HK2*vd + HK6*ve;
-	const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
-	const float HK10 = q0*q2;
-	const float HK11 = q1*q3;
-	const float HK12 = HK10 + HK11;
-	const float HK13 = 2*Tbs(1,2);
-	const float HK14 = q0*q3;
-	const float HK15 = q1*q2;
+	const float HK3 = q0*q1;
+	const float HK4 = q2*q3;
+	const float HK5 = HK3 + HK4;
+	const float HK6 = 2*HK5;
+	const float HK7 = q0*q2;
+	const float HK8 = q1*q3;
+	const float HK9 = HK7 - HK8;
+	const float HK10 = 2*HK9;
+	const float HK11 = powf(q3, 2);
+	const float HK12 = powf(q2, 2);
+	const float HK13 = -HK12;
+	const float HK14 = powf(q0, 2);
+	const float HK15 = powf(q1, 2);
 	const float HK16 = HK14 - HK15;
-	const float HK17 = 2*Tbs(1,1);
-	const float HK18 = ecl::powf(q1, 2);
-	const float HK19 = ecl::powf(q2, 2);
-	const float HK20 = -HK19;
-	const float HK21 = ecl::powf(q0, 2);
-	const float HK22 = ecl::powf(q3, 2);
-	const float HK23 = HK21 - HK22;
-	const float HK24 = HK18 + HK20 + HK23;
-	const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
-	const float HK26 = HK14 + HK15;
-	const float HK27 = 2*Tbs(1,0);
-	const float HK28 = q0*q1;
-	const float HK29 = q2*q3;
-	const float HK30 = HK28 - HK29;
-	const float HK31 = -HK18;
-	const float HK32 = HK19 + HK23 + HK31;
-	const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
-	const float HK34 = HK28 + HK29;
-	const float HK35 = HK10 - HK11;
-	const float HK36 = HK20 + HK21 + HK22 + HK31;
-	const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
-	const float HK38 = 2*HK3;
-	const float HK39 = 2*HK7;
-	const float HK40 = 2*HK8;
-	const float HK41 = 2*HK9;
-	const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
-	const float HK43 = ecl::powf(range, -2);
-	const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
-	const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
-	const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
-	const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
-	const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
-	const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
-	// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+	const float HK17 = HK11 + HK13 + HK16;
+	const float HK18 = -HK10*Tbs(2,0) + HK17*Tbs(2,2) + HK6*Tbs(2,1);
+	const float HK19 = -Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0;
+	const float HK20 = 2*Tbs(1,1);
+	const float HK21 = 2*Tbs(1,0);
+	const float HK22 = HK17*Tbs(1,2) + HK20*HK5 - HK21*HK9;
+	const float HK23 = q0*q3;
+	const float HK24 = q1*q2;
+	const float HK25 = HK23 + HK24;
+	const float HK26 = HK3 - HK4;
+	const float HK27 = 2*Tbs(1,2);
+	const float HK28 = -HK11;
+	const float HK29 = HK12 + HK16 + HK28;
+	const float HK30 = HK21*HK25 - HK26*HK27 + HK29*Tbs(1,1);
+	const float HK31 = HK7 + HK8;
+	const float HK32 = HK23 - HK24;
+	const float HK33 = HK13 + HK14 + HK15 + HK28;
+	const float HK34 = -HK20*HK32 + HK27*HK31 + HK33*Tbs(1,0);
+	const float HK35 = HK22*vd + HK30*ve + HK34*vn;
+	const float HK36 = HK18*(HK0*vd + HK1*ve + HK2*vn) + HK19*HK35;
+	const float HK37 = pd - ptd;
+	const float HK38 = 1.0F/HK37;
+	const float HK39 = 2*HK38;
+	const float HK40 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+	const float HK41 = Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1;
+	const float HK42 = HK18*(-HK0*ve + HK1*vd + HK40*vn) + HK35*HK41;
+	const float HK43 = Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2;
+	const float HK44 = -HK18*(HK0*vn - HK2*vd + HK40*ve) + HK35*HK43;
+	const float HK45 = Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3;
+	const float HK46 = HK18*(-HK1*vn + HK2*ve + HK40*vd) + HK35*HK45;
+	const float HK47 = HK18*HK38;
+	const float HK48 = powf(HK37, -2);
+	const float HK49 = HK18*HK48;
+	const float HK50 = HK35*HK49;
+	const float HK51 = HK18*HK34;
+	const float HK52 = HK51*P(0,4);
+	const float HK53 = HK18*HK30;
+	const float HK54 = HK53*P(0,5);
+	const float HK55 = HK18*HK22;
+	const float HK56 = HK55*P(0,6);
+	const float HK57 = HK35*HK47;
+	const float HK58 = HK57*P(0,24);
+	const float HK59 = HK57*P(0,9);
+	const float HK60 = 2*HK46;
+	const float HK61 = HK60*P(0,3);
+	const float HK62 = 2*HK36;
+	const float HK63 = HK62*P(0,0);
+	const float HK64 = 2*HK42;
+	const float HK65 = HK64*P(0,1);
+	const float HK66 = 2*HK44;
+	const float HK67 = HK66*P(0,2);
+	const float HK68 = HK51*P(4,6);
+	const float HK69 = HK53*P(5,6);
+	const float HK70 = HK55*P(6,6);
+	const float HK71 = HK57*P(6,9);
+	const float HK72 = HK57*P(6,24);
+	const float HK73 = HK60*P(3,6);
+	const float HK74 = HK62*P(0,6);
+	const float HK75 = HK64*P(1,6);
+	const float HK76 = HK66*P(2,6);
+	const float HK77 = HK51*P(4,5);
+	const float HK78 = HK53*P(5,5);
+	const float HK79 = HK55*P(5,6);
+	const float HK80 = HK57*P(5,9);
+	const float HK81 = HK57*P(5,24);
+	const float HK82 = HK60*P(3,5);
+	const float HK83 = HK62*P(0,5);
+	const float HK84 = HK64*P(1,5);
+	const float HK85 = HK66*P(2,5);
+	const float HK86 = HK51*P(4,4);
+	const float HK87 = HK53*P(4,5);
+	const float HK88 = HK55*P(4,6);
+	const float HK89 = HK57*P(4,9);
+	const float HK90 = HK57*P(4,24);
+	const float HK91 = HK60*P(3,4);
+	const float HK92 = HK62*P(0,4);
+	const float HK93 = HK64*P(1,4);
+	const float HK94 = HK66*P(2,4);
+	const float HK95 = HK51*P(4,24);
+	const float HK96 = HK53*P(5,24);
+	const float HK97 = HK55*P(6,24);
+	const float HK98 = HK57*P(9,24);
+	const float HK99 = HK57*P(24,24);
+	const float HK100 = HK60*P(3,24);
+	const float HK101 = HK62*P(0,24);
+	const float HK102 = HK64*P(1,24);
+	const float HK103 = HK66*P(2,24);
+	const float HK104 = HK18/powf(HK37, 3);
+	const float HK105 = HK104*HK35;
+	const float HK106 = HK51*P(4,9);
+	const float HK107 = HK53*P(5,9);
+	const float HK108 = HK55*P(6,9);
+	const float HK109 = HK57*P(9,9);
+	const float HK110 = -HK98;
+	const float HK111 = HK60*P(3,9);
+	const float HK112 = HK62*P(0,9);
+	const float HK113 = HK64*P(1,9);
+	const float HK114 = HK66*P(2,9);
+	const float HK115 = HK51*P(3,4);
+	const float HK116 = HK53*P(3,5);
+	const float HK117 = HK55*P(3,6);
+	const float HK118 = HK57*P(3,9);
+	const float HK119 = HK57*P(3,24);
+	const float HK120 = HK60*P(3,3);
+	const float HK121 = HK62*P(0,3);
+	const float HK122 = HK64*P(1,3);
+	const float HK123 = HK66*P(2,3);
+	const float HK124 = HK51*P(1,4);
+	const float HK125 = HK53*P(1,5);
+	const float HK126 = HK55*P(1,6);
+	const float HK127 = HK57*P(1,9);
+	const float HK128 = HK57*P(1,24);
+	const float HK129 = HK60*P(1,3);
+	const float HK130 = HK62*P(0,1);
+	const float HK131 = HK64*P(1,1);
+	const float HK132 = HK66*P(1,2);
+	const float HK133 = HK51*P(2,4);
+	const float HK134 = HK53*P(2,5);
+	const float HK135 = HK55*P(2,6);
+	const float HK136 = HK57*P(2,9);
+	const float HK137 = HK57*P(2,24);
+	const float HK138 = HK60*P(2,3);
+	const float HK139 = HK62*P(0,2);
+	const float HK140 = HK64*P(1,2);
+	const float HK141 = HK66*P(2,2);
+	// const float HK142 = HK38/(HK105*(-HK100 - HK101 - HK102 + HK103 - HK95 - HK96 - HK97 + HK98 - HK99) - HK105*(-HK106 - HK107 - HK108 + HK109 + HK110 - HK111 - HK112 - HK113 + HK114) + HK22*HK49*(-HK68 - HK69 - HK70 + HK71 - HK72 - HK73 - HK74 - HK75 + HK76) + HK30*HK49*(-HK77 - HK78 - HK79 + HK80 - HK81 - HK82 - HK83 - HK84 + HK85) + HK34*HK49*(-HK86 - HK87 - HK88 + HK89 - HK90 - HK91 - HK92 - HK93 + HK94) + HK48*HK60*(-HK115 - HK116 - HK117 + HK118 - HK119 - HK120 - HK121 - HK122 + HK123) + HK48*HK62*(-HK52 - HK54 - HK56 - HK58 + HK59 - HK61 - HK63 - HK65 + HK67) + HK48*HK64*(-HK124 - HK125 - HK126 + HK127 - HK128 - HK129 - HK130 - HK131 + HK132) - HK48*HK66*(-HK133 - HK134 - HK135 + HK136 - HK137 - HK138 - HK139 - HK140 + HK141) - R_LOS);
 
 	// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
-	_flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+	_flow_innov_var(0) = -(HK105*(-HK100 - HK101 - HK102 + HK103 - HK95 - HK96 - HK97 + HK98 - HK99) - HK105*(-HK106 - HK107 - HK108 + HK109 + HK110 - HK111 - HK112 - HK113 + HK114) + HK22*HK49*(-HK68 - HK69 - HK70 + HK71 - HK72 - HK73 - HK74 - HK75 + HK76) + HK30*HK49*(-HK77 - HK78 - HK79 + HK80 - HK81 - HK82 - HK83 - HK84 + HK85) + HK34*HK49*(-HK86 - HK87 - HK88 + HK89 - HK90 - HK91 - HK92 - HK93 + HK94) + HK48*HK60*(-HK115 - HK116 - HK117 + HK118 - HK119 - HK120 - HK121 - HK122 + HK123) + HK48*HK62*(-HK52 - HK54 - HK56 - HK58 + HK59 - HK61 - HK63 - HK65 + HK67) + HK48*HK64*(-HK124 - HK125 - HK126 + HK127 - HK128 - HK129 - HK130 - HK131 + HK132) - HK48*HK66*(-HK133 - HK134 - HK135 + HK136 - HK137 - HK138 - HK139 - HK140 + HK141) - R_LOS);
 
 	if (_flow_innov_var(0) < R_LOS) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		return;
 	}
-	const float HK50 = HK4/_flow_innov_var(0);
+	const float HK142 = HK38/(-_flow_innov_var(0));
 
-	const float HK51 = Tbs(0,1)*q1;
-	const float HK52 = Tbs(0,2)*q0;
-	const float HK53 = Tbs(0,0)*q2;
-	const float HK54 = HK51 + HK52 - HK53;
-	const float HK55 = Tbs(0,0)*q3;
-	const float HK56 = Tbs(0,1)*q0;
-	const float HK57 = Tbs(0,2)*q1;
-	const float HK58 = HK55 + HK56 - HK57;
-	const float HK59 = Tbs(0,0)*q0;
-	const float HK60 = Tbs(0,2)*q2;
-	const float HK61 = Tbs(0,1)*q3;
-	const float HK62 = HK59 + HK60 - HK61;
-	const float HK63 = HK54*vd + HK58*ve + HK62*vn;
-	const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
-	const float HK65 = HK58*vd + HK64*vn;
-	const float HK66 = -HK54*ve + HK65;
-	const float HK67 = HK54*vn + HK64*ve;
-	const float HK68 = -HK62*vd + HK67;
-	const float HK69 = HK62*ve + HK64*vd;
-	const float HK70 = -HK58*vn + HK69;
-	const float HK71 = 2*Tbs(0,1);
-	const float HK72 = 2*Tbs(0,2);
-	const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
-	const float HK74 = -HK16*HK71 + HK73;
-	const float HK75 = 2*Tbs(0,0);
-	const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
-	const float HK77 = -HK30*HK72 + HK76;
-	const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
-	const float HK79 = -HK35*HK75 + HK78;
-	const float HK80 = 2*HK63;
-	const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
-	const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
-	const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
-	const float HK84 = HK71*(-HK14 + HK15) + HK73;
-	const float HK85 = HK72*(-HK28 + HK29) + HK76;
-	const float HK86 = HK75*(-HK10 + HK11) + HK78;
-	const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
-	const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
-	const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
-	const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
-	const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
-	const float HK92 = 2*HK43;
-	const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
-	const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
-	// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+	const float HK143 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
+	const float HK144 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
+	const float HK145 = Tbs(0,0)*q0;
+	const float HK146 = Tbs(0,2)*q2;
+	const float HK147 = Tbs(0,1)*q3;
+	const float HK148 = HK145 + HK146 - HK147;
+	const float HK149 = HK17*Tbs(0,2) + HK6*Tbs(0,1);
+	const float HK150 = -HK10*Tbs(0,0) + HK149;
+	const float HK151 = 2*Tbs(0,2);
+	const float HK152 = 2*Tbs(0,0);
+	const float HK153 = HK152*HK25 + HK29*Tbs(0,1);
+	const float HK154 = -HK151*HK26 + HK153;
+	const float HK155 = 2*Tbs(0,1);
+	const float HK156 = HK151*HK31 + HK33*Tbs(0,0);
+	const float HK157 = -HK155*HK32 + HK156;
+	const float HK158 = HK150*vd + HK154*ve + HK157*vn;
+	const float HK159 = HK158*HK19 + HK18*(HK143*vd + HK144*ve + HK148*vn);
+	const float HK160 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+	const float HK161 = HK158*HK41 + HK18*(-HK143*ve + HK144*vd + HK160*vn);
+	const float HK162 = HK18*(HK143*vn + HK160*ve + vd*(-HK145 - HK146 + HK147));
+	const float HK163 = HK43*(vd*(HK149 + HK152*(-HK7 + HK8)) + ve*(HK151*(-HK3 + HK4) + HK153) + vn*(HK155*(-HK23 + HK24) + HK156));
+	const float HK164 = HK158*HK45 + HK18*(-HK144*vn + HK148*ve + HK160*vd);
+	const float HK165 = HK158*HK49;
+	const float HK166 = HK157*HK18;
+	const float HK167 = HK154*HK18;
+	const float HK168 = HK150*HK18;
+	const float HK169 = HK158*HK47;
+	const float HK170 = 2*HK164;
+	const float HK171 = 2*HK159;
+	const float HK172 = 2*HK161;
+	const float HK173 = -2*HK162 + 2*HK163;
+	const float HK174 = HK166*P(0,4) + HK167*P(0,5) + HK168*P(0,6) + HK169*P(0,24) - HK169*P(0,9) + HK170*P(0,3) + HK171*P(0,0) + HK172*P(0,1) - HK173*P(0,2);
+	const float HK175 = HK166*P(4,6) + HK167*P(5,6) + HK168*P(6,6) + HK169*P(6,24) - HK169*P(6,9) + HK170*P(3,6) + HK171*P(0,6) + HK172*P(1,6) - HK173*P(2,6);
+	const float HK176 = HK166*P(4,5) + HK167*P(5,5) + HK168*P(5,6) + HK169*P(5,24) - HK169*P(5,9) + HK170*P(3,5) + HK171*P(0,5) + HK172*P(1,5) - HK173*P(2,5);
+	const float HK177 = HK166*P(4,4) + HK167*P(4,5) + HK168*P(4,6) + HK169*P(4,24) - HK169*P(4,9) + HK170*P(3,4) + HK171*P(0,4) + HK172*P(1,4) - HK173*P(2,4);
+	const float HK178 = HK169*P(9,24);
+	const float HK179 = HK166*P(4,24) + HK167*P(5,24) + HK168*P(6,24) + HK169*P(24,24) + HK170*P(3,24) + HK171*P(0,24) + HK172*P(1,24) - HK173*P(2,24) - HK178;
+	const float HK180 = HK104*HK158;
+	const float HK181 = HK166*P(4,9) + HK167*P(5,9) + HK168*P(6,9) - HK169*P(9,9) + HK170*P(3,9) + HK171*P(0,9) + HK172*P(1,9) - HK173*P(2,9) + HK178;
+	const float HK182 = HK166*P(3,4) + HK167*P(3,5) + HK168*P(3,6) + HK169*P(3,24) - HK169*P(3,9) + HK170*P(3,3) + HK171*P(0,3) + HK172*P(1,3) - HK173*P(2,3);
+	const float HK183 = HK166*P(1,4) + HK167*P(1,5) + HK168*P(1,6) + HK169*P(1,24) - HK169*P(1,9) + HK170*P(1,3) + HK171*P(0,1) + HK172*P(1,1) - HK173*P(1,2);
+	const float HK184 = HK166*P(2,4) + HK167*P(2,5) + HK168*P(2,6) + HK169*P(2,24) - HK169*P(2,9) + HK170*P(2,3) + HK171*P(0,2) + HK172*P(1,2) - HK173*P(2,2);
+	// const float HK185 = HK38/(HK150*HK175*HK49 + HK154*HK176*HK49 + HK157*HK177*HK49 + HK170*HK182*HK48 + HK171*HK174*HK48 + HK172*HK183*HK48 - HK173*HK184*HK48 + HK179*HK180 - HK180*HK181 + R_LOS);
 
 	// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
-	_flow_innov_var(1) = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+	_flow_innov_var(1) = (HK150*HK175*HK49 + HK154*HK176*HK49 + HK157*HK177*HK49 + HK170*HK182*HK48 + HK171*HK174*HK48 + HK172*HK183*HK48 - HK173*HK184*HK48 + HK179*HK180 - HK180*HK181 + R_LOS);
 	if (_flow_innov_var(1) < R_LOS) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		return;
 	}
-	const float HK95 = HK4/_flow_innov_var(1);
+	const float HK185 = HK38/_flow_innov_var(1);
 
 
 	// run the innovation consistency check and record result
@@ -260,58 +354,100 @@ void Ekf::fuseOptFlow()
 	}
 
 	// fuse observation axes sequentially
-	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
-	Vector24f Kfusion; // Optical flow Kalman gains
+	SparseVector25f<0,1,2,3,4,5,6,9,24> Hfusion; // Optical flow observation Jacobians
+	Vector25f Kfusion; // Optical flow Kalman gains
 
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
 
 		// calculate observation Jocobians and Kalman gains
 		if (obs_index == 0) {
 			// Observation Jacobians - axis 0
-			Hfusion.at<0>() = HK3*HK5;
-			Hfusion.at<1>() = HK5*HK7;
-			Hfusion.at<2>() = HK5*HK8;
-			Hfusion.at<3>() = HK5*HK9;
-			Hfusion.at<4>() = HK25*HK4;
-			Hfusion.at<5>() = HK33*HK4;
-			Hfusion.at<6>() = HK37*HK4;
+			Hfusion.at<0>() = -HK36*HK39;
+			Hfusion.at<1>() = -HK39*HK42;
+			Hfusion.at<2>() = HK39*HK44;
+			Hfusion.at<3>() = -HK39*HK46;
+			Hfusion.at<4>() = -HK34*HK47;
+			Hfusion.at<5>() = -HK30*HK47;
+			Hfusion.at<6>() = -HK22*HK47;
+			Hfusion.at<9>() = HK50;
+			Hfusion.at<24>() = -HK50;
 
-			// Kalman gains - axis 0
-			Kfusion(0) = HK42*HK50;
-			Kfusion(1) = HK49*HK50;
-			Kfusion(2) = HK47*HK50;
-			Kfusion(3) = HK48*HK50;
-			Kfusion(4) = HK46*HK50;
-			Kfusion(5) = HK45*HK50;
-			Kfusion(6) = HK44*HK50;
+			if (_control_status.flags.opt_flow) {
+				// Kalman gains - axis 0
+				Kfusion(0) = HK142*(HK52 + HK54 + HK56 + HK58 - HK59 + HK61 + HK63 + HK65 - HK67);
+				Kfusion(1) = HK142*(HK124 + HK125 + HK126 - HK127 + HK128 + HK129 + HK130 + HK131 - HK132);
+				Kfusion(2) = HK142*(HK133 + HK134 + HK135 - HK136 + HK137 + HK138 + HK139 + HK140 - HK141);
+				Kfusion(3) = HK142*(HK115 + HK116 + HK117 - HK118 + HK119 + HK120 + HK121 + HK122 - HK123);
+				Kfusion(4) = HK142*(HK86 + HK87 + HK88 - HK89 + HK90 + HK91 + HK92 + HK93 - HK94);
+				Kfusion(5) = HK142*(HK77 + HK78 + HK79 - HK80 + HK81 + HK82 + HK83 + HK84 - HK85);
+				Kfusion(6) = HK142*(HK68 + HK69 + HK70 - HK71 + HK72 + HK73 + HK74 + HK75 - HK76);
+				Kfusion(7) = HK142*(HK51*P(4,7) + HK53*P(5,7) + HK55*P(6,7) + HK57*P(7,24) - HK57*P(7,9) + HK60*P(3,7) + HK62*P(0,7) + HK64*P(1,7) - HK66*P(2,7));
+				Kfusion(8) = HK142*(HK51*P(4,8) + HK53*P(5,8) + HK55*P(6,8) + HK57*P(8,24) - HK57*P(8,9) + HK60*P(3,8) + HK62*P(0,8) + HK64*P(1,8) - HK66*P(2,8));
+				/* Kfusion(9) = HK142*(HK106 + HK107 + HK108 - HK109 + HK111 + HK112 + HK113 - HK114 + HK98); */
+				Kfusion(9) = 0.f;
 
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
+				for (unsigned row = 10; row <= 23; row++) {
+					Kfusion(row) = HK142*(HK51*P(4,row) + HK53*P(5,row) + HK55*P(6,row) + HK57*P(row,24) - HK57*P(9,row) + HK60*P(3,row) + HK62*P(0,row) + HK64*P(1,row) - HK66*P(2,row));
+				}
+
+			} else {
+				for (unsigned row = 0; row <= 23; row++) {
+					// update of all states other than terrain is inhibited
+					Kfusion(row) = 0.0f;
+				}
+			}
+
+			if (_hagl_sensor_status.flags.flow) {
+				Kfusion(24) = HK142*(HK100 + HK101 + HK102 - HK103 + HK110 + HK95 + HK96 + HK97 + HK99);
+
+			} else {
+				Kfusion(24) = 0.f;
 			}
 
 		} else {
 			// Observation Jacobians - axis 1
-			Hfusion.at<0>() = -HK5*HK63;
-			Hfusion.at<1>() = -HK5*HK66;
-			Hfusion.at<2>() = -HK5*HK68;
-			Hfusion.at<3>() = -HK5*HK70;
-			Hfusion.at<4>() = -HK4*HK74;
-			Hfusion.at<5>() = -HK4*HK77;
-			Hfusion.at<6>() = -HK4*HK79;
+			Hfusion.at<0>() = HK159*HK39;
+			Hfusion.at<1>() = HK161*HK39;
+			Hfusion.at<2>() = HK39*(HK162 - HK163);
+			Hfusion.at<3>() = HK164*HK39;
+			Hfusion.at<4>() = HK157*HK47;
+			Hfusion.at<5>() = HK154*HK47;
+			Hfusion.at<6>() = HK150*HK47;
+			Hfusion.at<9>() = -HK165;
+			Hfusion.at<24>() = HK165;
 
-			// Kalman gains - axis 1
-			Kfusion(0) = -HK87*HK95;
-			Kfusion(1) = -HK94*HK95;
-			Kfusion(2) = -HK91*HK95;
-			Kfusion(3) = -HK93*HK95;
-			Kfusion(4) = -HK90*HK95;
-			Kfusion(5) = -HK89*HK95;
-			Kfusion(6) = -HK88*HK95;
+			if (_control_status.flags.opt_flow) {
+				// Kalman gains - axis 1
+				Kfusion(0) = HK174*HK185;
+				Kfusion(1) = HK183*HK185;
+				Kfusion(2) = HK184*HK185;
+				Kfusion(3) = HK182*HK185;
+				Kfusion(4) = HK177*HK185;
+				Kfusion(5) = HK176*HK185;
+				Kfusion(6) = HK175*HK185;
+				Kfusion(7) = HK185*(HK166*P(4,7) + HK167*P(5,7) + HK168*P(6,7) + HK169*P(7,24) - HK169*P(7,9) + HK170*P(3,7) + HK171*P(0,7) + HK172*P(1,7) - HK173*P(2,7));
+				Kfusion(8) = HK185*(HK166*P(4,8) + HK167*P(5,8) + HK168*P(6,8) + HK169*P(8,24) - HK169*P(8,9) + HK170*P(3,8) + HK171*P(0,8) + HK172*P(1,8) - HK173*P(2,8));
+				/* Kfusion(9) = HK181*HK185; */
+				Kfusion(9) = 0.f;
 
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
+
+				for (unsigned row = 10; row <= 23; row++) {
+					Kfusion(row) = HK185*(HK166*P(4,row) + HK167*P(5,row) + HK168*P(6,row) + HK169*P(row,24) - HK169*P(9,row) + HK170*P(3,row) + HK171*P(0,row) + HK172*P(1,row) - HK173*P(2,row));
+				}
+
+			} else {
+				for (unsigned row = 0; row <= 23; row++) {
+					// update of all states other than terrain is inhibited
+					Kfusion(row) = 0.0f;
+				}
 			}
 
+			if (_hagl_sensor_status.flags.flow) {
+				Kfusion(24) = HK179*HK185;
+
+			} else {
+				Kfusion(24) = 0.f;
+			}
 		}
 
 		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _flow_innov(obs_index));

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp

@@ -3,8 +3,8 @@
 #include <cstdlib>
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 float sq(float in) {
     return in * in;
@@ -15,8 +15,8 @@ int main()
     // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
 
     float Hfusion[24];
-    Vector24f H_MAG;
-    Vector24f Kfusion;
+    Vector25f H_MAG;
+    Vector25f Kfusion;
     float mag_innov_var;
 
     // quaternion inputs must be normalised
@@ -38,7 +38,7 @@ int main()
     const bool update_all_states = true;
 
     // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {
@@ -132,7 +132,7 @@ int main()
 
         // save output and repeat calculation using legacy matlab generated code
         float Hfusion_sympy[24];
-        Vector24f Kfusion_sympy;
+        Vector25f Kfusion_sympy;
         for (int row=0; row<24; row++) {
             Hfusion_sympy[row] = Hfusion[row];
             Kfusion_sympy(row) = Kfusion(row);
@@ -308,7 +308,7 @@ int main()
 
         // save output and repeat calculation using legacy matlab generated code
         float Hfusion_sympy[24];
-        Vector24f Kfusion_sympy;
+        Vector25f Kfusion_sympy;
         for (int row=0; row<24; row++) {
             Hfusion_sympy[row] = Hfusion[row];
             Kfusion_sympy(row) = Kfusion(row);
@@ -488,7 +488,7 @@ int main()
 
         // save output and repeat calculation using legacy matlab generated code
         float Hfusion_sympy[24];
-        Vector24f Kfusion_sympy;
+        Vector25f Kfusion_sympy;
         for (int row=0; row<24; row++) {
             Hfusion_sympy[row] = Hfusion[row];
             Kfusion_sympy(row) = Kfusion(row);

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp

@@ -1,83 +1,37 @@
 // Axis 0 equations
 // Sub Expressions
-const float HKX0 = magE*q3;
-const float HKX1 = magD*q2;
-const float HKX2 = magD*q3 + magE*q2;
-const float HKX3 = magE*q1;
-const float HKX4 = magD*q0;
-const float HKX5 = 2*magN;
-const float HKX6 = HKX5*q2;
-const float HKX7 = -HKX5*q3 + magD*q1 + magE*q0;
-const float HKX8 = 2*powf(q2, 2);
-const float HKX9 = 2*powf(q3, 2);
-const float HKX10 = q0*q3 + q1*q2;
-const float HKX11 = q1*q3;
-const float HKX12 = q0*q2;
-const float HKX13 = 2*HKX2;
-const float HKX14 = HKX13*P(0,1);
-const float HKX15 = 2*HKX10;
-const float HKX16 = HKX15*P(0,17);
-const float HKX17 = -2*HKX0 + 2*HKX1;
-const float HKX18 = HKX17*P(0,0);
-const float HKX19 = -2*HKX11 + 2*HKX12;
-const float HKX20 = HKX19*P(0,18);
-const float HKX21 = HKX8 + HKX9 - 1;
-const float HKX22 = HKX21*P(0,16);
-const float HKX23 = 2*HKX7;
-const float HKX24 = HKX23*P(0,3);
-const float HKX25 = -2*HKX3 + 2*HKX4 + 2*HKX6;
-const float HKX26 = HKX25*P(0,2);
-const float HKX27 = HKX13*P(1,16);
-const float HKX28 = HKX15*P(16,17);
-const float HKX29 = HKX17*P(0,16);
-const float HKX30 = HKX19*P(16,18);
-const float HKX31 = HKX23*P(3,16);
-const float HKX32 = HKX21*P(16,16);
-const float HKX33 = HKX25*P(2,16);
-const float HKX34 = HKX13*P(1,1);
-const float HKX35 = HKX15*P(1,17);
-const float HKX36 = HKX17*P(0,1);
-const float HKX37 = HKX19*P(1,18);
-const float HKX38 = HKX23*P(1,3);
-const float HKX39 = HKX21*P(1,16);
-const float HKX40 = HKX25*P(1,2);
-const float HKX41 = HKX13*P(1,17);
-const float HKX42 = HKX15*P(17,17);
-const float HKX43 = HKX17*P(0,17);
-const float HKX44 = HKX19*P(17,18);
-const float HKX45 = HKX23*P(3,17);
-const float HKX46 = HKX21*P(16,17);
-const float HKX47 = HKX25*P(2,17);
-const float HKX48 = HKX13*P(1,3);
-const float HKX49 = HKX15*P(3,17);
-const float HKX50 = HKX17*P(0,3);
-const float HKX51 = HKX19*P(3,18);
-const float HKX52 = HKX23*P(3,3);
-const float HKX53 = HKX25*P(2,3);
-const float HKX54 = HKX21*P(3,16);
-const float HKX55 = HKX13*P(1,18);
-const float HKX56 = HKX15*P(17,18);
-const float HKX57 = HKX17*P(0,18);
-const float HKX58 = HKX19*P(18,18);
-const float HKX59 = HKX23*P(3,18);
-const float HKX60 = HKX21*P(16,18);
-const float HKX61 = HKX25*P(2,18);
-const float HKX62 = HKX13*P(1,2);
-const float HKX63 = HKX15*P(2,17);
-const float HKX64 = HKX17*P(0,2);
-const float HKX65 = HKX19*P(2,18);
-const float HKX66 = HKX23*P(2,3);
-const float HKX67 = HKX21*P(2,16);
-const float HKX68 = HKX25*P(2,2);
-const float HKX69 = -HKX13*P(1,19) - HKX15*P(17,19) + HKX17*P(0,19) + HKX19*P(18,19) + HKX21*P(16,19) - HKX23*P(3,19) + HKX25*P(2,19) - P(19,19);
-const float HKX70 = 1.0F/(-HKX13*(HKX34 + HKX35 - HKX36 - HKX37 + HKX38 - HKX39 - HKX40 + P(1,19)) - HKX15*(HKX41 + HKX42 - HKX43 - HKX44 + HKX45 - HKX46 - HKX47 + P(17,19)) + HKX17*(HKX14 + HKX16 - HKX18 - HKX20 - HKX22 + HKX24 - HKX26 + P(0,19)) + HKX19*(HKX55 + HKX56 - HKX57 - HKX58 + HKX59 - HKX60 - HKX61 + P(18,19)) + HKX21*(HKX27 + HKX28 - HKX29 - HKX30 + HKX31 - HKX32 - HKX33 + P(16,19)) - HKX23*(HKX48 + HKX49 - HKX50 - HKX51 + HKX52 - HKX53 - HKX54 + P(3,19)) + HKX25*(HKX62 + HKX63 - HKX64 - HKX65 + HKX66 - HKX67 - HKX68 + P(2,19)) + HKX69 - R_MAG);
+const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
+const float HKX1 = magD*q3 + magE*q2 + magN*q1;
+const float HKX2 = magE*q1;
+const float HKX3 = magD*q0;
+const float HKX4 = magN*q2;
+const float HKX5 = magD*q1 + magE*q0 - magN*q3;
+const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
+const float HKX7 = q0*q3 + q1*q2;
+const float HKX8 = q1*q3;
+const float HKX9 = q0*q2;
+const float HKX10 = 2*HKX7;
+const float HKX11 = -2*HKX8 + 2*HKX9;
+const float HKX12 = 2*HKX1;
+const float HKX13 = 2*HKX0;
+const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4;
+const float HKX15 = 2*HKX5;
+const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19);
+const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19);
+const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19);
+const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19);
+const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19);
+const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19);
+const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
+const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
+const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = 2*HKX0 - 2*HKX1;
-Hfusion.at<1>() = 2*HKX2;
-Hfusion.at<2>() = 2*HKX3 - 2*HKX4 - 2*HKX6;
-Hfusion.at<3>() = 2*HKX7;
+Hfusion.at<0>() = 2*HKX0;
+Hfusion.at<1>() = 2*HKX1;
+Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
+Hfusion.at<3>() = 2*HKX5;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -90,80 +44,79 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = -HKX8 - HKX9 + 1;
-Hfusion.at<17>() = 2*HKX10;
-Hfusion.at<18>() = 2*HKX11 - 2*HKX12;
+Hfusion.at<16>() = HKX6;
+Hfusion.at<17>() = 2*HKX7;
+Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
 Hfusion.at<19>() = 1;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HKX70*(-HKX14 - HKX16 + HKX18 + HKX20 + HKX22 - HKX24 + HKX26 - P(0,19));
-Kfusion(1) = HKX70*(-HKX34 - HKX35 + HKX36 + HKX37 - HKX38 + HKX39 + HKX40 - P(1,19));
-Kfusion(2) = HKX70*(-HKX62 - HKX63 + HKX64 + HKX65 - HKX66 + HKX67 + HKX68 - P(2,19));
-Kfusion(3) = HKX70*(-HKX48 - HKX49 + HKX50 + HKX51 - HKX52 + HKX53 + HKX54 - P(3,19));
-Kfusion(4) = HKX70*(-HKX13*P(1,4) - HKX15*P(4,17) + HKX17*P(0,4) + HKX19*P(4,18) + HKX21*P(4,16) - HKX23*P(3,4) + HKX25*P(2,4) - P(4,19));
-Kfusion(5) = HKX70*(-HKX13*P(1,5) - HKX15*P(5,17) + HKX17*P(0,5) + HKX19*P(5,18) + HKX21*P(5,16) - HKX23*P(3,5) + HKX25*P(2,5) - P(5,19));
-Kfusion(6) = HKX70*(-HKX13*P(1,6) - HKX15*P(6,17) + HKX17*P(0,6) + HKX19*P(6,18) + HKX21*P(6,16) - HKX23*P(3,6) + HKX25*P(2,6) - P(6,19));
-Kfusion(7) = HKX70*(-HKX13*P(1,7) - HKX15*P(7,17) + HKX17*P(0,7) + HKX19*P(7,18) + HKX21*P(7,16) - HKX23*P(3,7) + HKX25*P(2,7) - P(7,19));
-Kfusion(8) = HKX70*(-HKX13*P(1,8) - HKX15*P(8,17) + HKX17*P(0,8) + HKX19*P(8,18) + HKX21*P(8,16) - HKX23*P(3,8) + HKX25*P(2,8) - P(8,19));
-Kfusion(9) = HKX70*(-HKX13*P(1,9) - HKX15*P(9,17) + HKX17*P(0,9) + HKX19*P(9,18) + HKX21*P(9,16) - HKX23*P(3,9) + HKX25*P(2,9) - P(9,19));
-Kfusion(10) = HKX70*(-HKX13*P(1,10) - HKX15*P(10,17) + HKX17*P(0,10) + HKX19*P(10,18) + HKX21*P(10,16) - HKX23*P(3,10) + HKX25*P(2,10) - P(10,19));
-Kfusion(11) = HKX70*(-HKX13*P(1,11) - HKX15*P(11,17) + HKX17*P(0,11) + HKX19*P(11,18) + HKX21*P(11,16) - HKX23*P(3,11) + HKX25*P(2,11) - P(11,19));
-Kfusion(12) = HKX70*(-HKX13*P(1,12) - HKX15*P(12,17) + HKX17*P(0,12) + HKX19*P(12,18) + HKX21*P(12,16) - HKX23*P(3,12) + HKX25*P(2,12) - P(12,19));
-Kfusion(13) = HKX70*(-HKX13*P(1,13) - HKX15*P(13,17) + HKX17*P(0,13) + HKX19*P(13,18) + HKX21*P(13,16) - HKX23*P(3,13) + HKX25*P(2,13) - P(13,19));
-Kfusion(14) = HKX70*(-HKX13*P(1,14) - HKX15*P(14,17) + HKX17*P(0,14) + HKX19*P(14,18) + HKX21*P(14,16) - HKX23*P(3,14) + HKX25*P(2,14) - P(14,19));
-Kfusion(15) = HKX70*(-HKX13*P(1,15) - HKX15*P(15,17) + HKX17*P(0,15) + HKX19*P(15,18) + HKX21*P(15,16) - HKX23*P(3,15) + HKX25*P(2,15) - P(15,19));
-Kfusion(16) = HKX70*(-HKX27 - HKX28 + HKX29 + HKX30 - HKX31 + HKX32 + HKX33 - P(16,19));
-Kfusion(17) = HKX70*(-HKX41 - HKX42 + HKX43 + HKX44 - HKX45 + HKX46 + HKX47 - P(17,19));
-Kfusion(18) = HKX70*(-HKX55 - HKX56 + HKX57 + HKX58 - HKX59 + HKX60 + HKX61 - P(18,19));
-Kfusion(19) = HKX69*HKX70;
-Kfusion(20) = HKX70*(-HKX13*P(1,20) - HKX15*P(17,20) + HKX17*P(0,20) + HKX19*P(18,20) + HKX21*P(16,20) - HKX23*P(3,20) + HKX25*P(2,20) - P(19,20));
-Kfusion(21) = HKX70*(-HKX13*P(1,21) - HKX15*P(17,21) + HKX17*P(0,21) + HKX19*P(18,21) + HKX21*P(16,21) - HKX23*P(3,21) + HKX25*P(2,21) - P(19,21));
-Kfusion(22) = HKX70*(-HKX13*P(1,22) - HKX15*P(17,22) + HKX17*P(0,22) + HKX19*P(18,22) + HKX21*P(16,22) - HKX23*P(3,22) + HKX25*P(2,22) - P(19,22));
-Kfusion(23) = HKX70*(-HKX13*P(1,23) - HKX15*P(17,23) + HKX17*P(0,23) + HKX19*P(18,23) + HKX21*P(16,23) - HKX23*P(3,23) + HKX25*P(2,23) - P(19,23));
+Kfusion(0) = HKX16*HKX24;
+Kfusion(1) = HKX22*HKX24;
+Kfusion(2) = HKX19*HKX24;
+Kfusion(3) = HKX21*HKX24;
+Kfusion(4) = HKX24*(HKX10*P(4,17) - HKX11*P(4,18) + HKX12*P(1,4) + HKX13*P(0,4) - HKX14*P(2,4) + HKX15*P(3,4) + HKX6*P(4,16) + P(4,19));
+Kfusion(5) = HKX24*(HKX10*P(5,17) - HKX11*P(5,18) + HKX12*P(1,5) + HKX13*P(0,5) - HKX14*P(2,5) + HKX15*P(3,5) + HKX6*P(5,16) + P(5,19));
+Kfusion(6) = HKX24*(HKX10*P(6,17) - HKX11*P(6,18) + HKX12*P(1,6) + HKX13*P(0,6) - HKX14*P(2,6) + HKX15*P(3,6) + HKX6*P(6,16) + P(6,19));
+Kfusion(7) = HKX24*(HKX10*P(7,17) - HKX11*P(7,18) + HKX12*P(1,7) + HKX13*P(0,7) - HKX14*P(2,7) + HKX15*P(3,7) + HKX6*P(7,16) + P(7,19));
+Kfusion(8) = HKX24*(HKX10*P(8,17) - HKX11*P(8,18) + HKX12*P(1,8) + HKX13*P(0,8) - HKX14*P(2,8) + HKX15*P(3,8) + HKX6*P(8,16) + P(8,19));
+Kfusion(9) = HKX24*(HKX10*P(9,17) - HKX11*P(9,18) + HKX12*P(1,9) + HKX13*P(0,9) - HKX14*P(2,9) + HKX15*P(3,9) + HKX6*P(9,16) + P(9,19));
+Kfusion(10) = HKX24*(HKX10*P(10,17) - HKX11*P(10,18) + HKX12*P(1,10) + HKX13*P(0,10) - HKX14*P(2,10) + HKX15*P(3,10) + HKX6*P(10,16) + P(10,19));
+Kfusion(11) = HKX24*(HKX10*P(11,17) - HKX11*P(11,18) + HKX12*P(1,11) + HKX13*P(0,11) - HKX14*P(2,11) + HKX15*P(3,11) + HKX6*P(11,16) + P(11,19));
+Kfusion(12) = HKX24*(HKX10*P(12,17) - HKX11*P(12,18) + HKX12*P(1,12) + HKX13*P(0,12) - HKX14*P(2,12) + HKX15*P(3,12) + HKX6*P(12,16) + P(12,19));
+Kfusion(13) = HKX24*(HKX10*P(13,17) - HKX11*P(13,18) + HKX12*P(1,13) + HKX13*P(0,13) - HKX14*P(2,13) + HKX15*P(3,13) + HKX6*P(13,16) + P(13,19));
+Kfusion(14) = HKX24*(HKX10*P(14,17) - HKX11*P(14,18) + HKX12*P(1,14) + HKX13*P(0,14) - HKX14*P(2,14) + HKX15*P(3,14) + HKX6*P(14,16) + P(14,19));
+Kfusion(15) = HKX24*(HKX10*P(15,17) - HKX11*P(15,18) + HKX12*P(1,15) + HKX13*P(0,15) - HKX14*P(2,15) + HKX15*P(3,15) + HKX6*P(15,16) + P(15,19));
+Kfusion(16) = HKX17*HKX24;
+Kfusion(17) = HKX20*HKX24;
+Kfusion(18) = HKX18*HKX24;
+Kfusion(19) = HKX23*HKX24;
+Kfusion(20) = HKX24*(HKX10*P(17,20) - HKX11*P(18,20) + HKX12*P(1,20) + HKX13*P(0,20) - HKX14*P(2,20) + HKX15*P(3,20) + HKX6*P(16,20) + P(19,20));
+Kfusion(21) = HKX24*(HKX10*P(17,21) - HKX11*P(18,21) + HKX12*P(1,21) + HKX13*P(0,21) - HKX14*P(2,21) + HKX15*P(3,21) + HKX6*P(16,21) + P(19,21));
+Kfusion(22) = HKX24*(HKX10*P(17,22) - HKX11*P(18,22) + HKX12*P(1,22) + HKX13*P(0,22) - HKX14*P(2,22) + HKX15*P(3,22) + HKX6*P(16,22) + P(19,22));
+Kfusion(23) = HKX24*(HKX10*P(17,23) - HKX11*P(18,23) + HKX12*P(1,23) + HKX13*P(0,23) - HKX14*P(2,23) + HKX15*P(3,23) + HKX6*P(16,23) + P(19,23));
+Kfusion(24) = HKX24*(HKX10*P(17,24) - HKX11*P(18,24) + HKX12*P(1,24) + HKX13*P(0,24) - HKX14*P(2,24) + HKX15*P(3,24) + HKX6*P(16,24) + P(19,24));
 
 
 // Axis 1 equations
 // Sub Expressions
-const float HKY0 = magD*q1 - magN*q3;
-const float HKY1 = 2*magE;
-const float HKY2 = -HKY1*q1 + magD*q0 + magN*q2;
-const float HKY3 = magD*q3 + magN*q1;
-const float HKY4 = magD*q2;
-const float HKY5 = HKY1*q3;
-const float HKY6 = magN*q0;
-const float HKY7 = q1*q2;
-const float HKY8 = q0*q3;
-const float HKY9 = 2*powf(q1, 2);
-const float HKY10 = 2*powf(q3, 2);
-const float HKY11 = q0*q1 + q2*q3;
-const float HKY12 = 2*HKY11;
-const float HKY13 = 2*HKY3;
-const float HKY14 = 2*HKY0;
-const float HKY15 = -2*HKY7 + 2*HKY8;
-const float HKY16 = 2*HKY2;
-const float HKY17 = HKY10 + HKY9 - 1;
-const float HKY18 = -2*HKY4 + 2*HKY5 + 2*HKY6;
-const float HKY19 = HKY12*P(0,18) + HKY13*P(0,2) + HKY14*P(0,0) - HKY15*P(0,16) + HKY16*P(0,1) - HKY17*P(0,17) - HKY18*P(0,3) + P(0,20);
-const float HKY20 = HKY12*P(17,18) + HKY13*P(2,17) + HKY14*P(0,17) - HKY15*P(16,17) + HKY16*P(1,17) - HKY17*P(17,17) - HKY18*P(3,17) + P(17,20);
-const float HKY21 = HKY12*P(16,18) + HKY13*P(2,16) + HKY14*P(0,16) - HKY15*P(16,16) + HKY16*P(1,16) - HKY17*P(16,17) - HKY18*P(3,16) + P(16,20);
-const float HKY22 = HKY12*P(3,18) + HKY13*P(2,3) + HKY14*P(0,3) - HKY15*P(3,16) + HKY16*P(1,3) - HKY17*P(3,17) - HKY18*P(3,3) + P(3,20);
-const float HKY23 = HKY12*P(2,18) + HKY13*P(2,2) + HKY14*P(0,2) - HKY15*P(2,16) + HKY16*P(1,2) - HKY17*P(2,17) - HKY18*P(2,3) + P(2,20);
-const float HKY24 = HKY12*P(18,18) + HKY13*P(2,18) + HKY14*P(0,18) - HKY15*P(16,18) + HKY16*P(1,18) - HKY17*P(17,18) - HKY18*P(3,18) + P(18,20);
-const float HKY25 = HKY12*P(1,18) + HKY13*P(1,2) + HKY14*P(0,1) - HKY15*P(1,16) + HKY16*P(1,1) - HKY17*P(1,17) - HKY18*P(1,3) + P(1,20);
-const float HKY26 = HKY12*P(18,20) + HKY13*P(2,20) + HKY14*P(0,20) - HKY15*P(16,20) + HKY16*P(1,20) - HKY17*P(17,20) - HKY18*P(3,20) + P(20,20);
-const float HKY27 = 1.0F/(HKY12*HKY24 + HKY13*HKY23 + HKY14*HKY19 - HKY15*HKY21 + HKY16*HKY25 - HKY17*HKY20 - HKY18*HKY22 + HKY26 + R_MAG);
+const float HKY0 = magD*q1 + magE*q0 - magN*q3;
+const float HKY1 = magD*q0 - magE*q1 + magN*q2;
+const float HKY2 = magD*q3 + magE*q2 + magN*q1;
+const float HKY3 = magD*q2;
+const float HKY4 = magE*q3;
+const float HKY5 = magN*q0;
+const float HKY6 = q1*q2;
+const float HKY7 = q0*q3;
+const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
+const float HKY9 = q0*q1 + q2*q3;
+const float HKY10 = 2*HKY9;
+const float HKY11 = -2*HKY6 + 2*HKY7;
+const float HKY12 = 2*HKY2;
+const float HKY13 = 2*HKY0;
+const float HKY14 = 2*HKY1;
+const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
+const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
+const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
+const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
+const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
+const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
+const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
+const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
+const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
+const float HKY24 = 1.0F/(HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
 
 
 // Observation Jacobians
 Hfusion.at<0>() = 2*HKY0;
-Hfusion.at<1>() = 2*HKY2;
-Hfusion.at<2>() = 2*HKY3;
-Hfusion.at<3>() = 2*HKY4 - 2*HKY5 - 2*HKY6;
+Hfusion.at<1>() = 2*HKY1;
+Hfusion.at<2>() = 2*HKY2;
+Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -176,123 +129,79 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = 2*HKY7 - 2*HKY8;
-Hfusion.at<17>() = -HKY10 - HKY9 + 1;
-Hfusion.at<18>() = 2*HKY11;
+Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
+Hfusion.at<17>() = HKY8;
+Hfusion.at<18>() = 2*HKY9;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 1;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HKY19*HKY27;
-Kfusion(1) = HKY25*HKY27;
-Kfusion(2) = HKY23*HKY27;
-Kfusion(3) = HKY22*HKY27;
-Kfusion(4) = HKY27*(HKY12*P(4,18) + HKY13*P(2,4) + HKY14*P(0,4) - HKY15*P(4,16) + HKY16*P(1,4) - HKY17*P(4,17) - HKY18*P(3,4) + P(4,20));
-Kfusion(5) = HKY27*(HKY12*P(5,18) + HKY13*P(2,5) + HKY14*P(0,5) - HKY15*P(5,16) + HKY16*P(1,5) - HKY17*P(5,17) - HKY18*P(3,5) + P(5,20));
-Kfusion(6) = HKY27*(HKY12*P(6,18) + HKY13*P(2,6) + HKY14*P(0,6) - HKY15*P(6,16) + HKY16*P(1,6) - HKY17*P(6,17) - HKY18*P(3,6) + P(6,20));
-Kfusion(7) = HKY27*(HKY12*P(7,18) + HKY13*P(2,7) + HKY14*P(0,7) - HKY15*P(7,16) + HKY16*P(1,7) - HKY17*P(7,17) - HKY18*P(3,7) + P(7,20));
-Kfusion(8) = HKY27*(HKY12*P(8,18) + HKY13*P(2,8) + HKY14*P(0,8) - HKY15*P(8,16) + HKY16*P(1,8) - HKY17*P(8,17) - HKY18*P(3,8) + P(8,20));
-Kfusion(9) = HKY27*(HKY12*P(9,18) + HKY13*P(2,9) + HKY14*P(0,9) - HKY15*P(9,16) + HKY16*P(1,9) - HKY17*P(9,17) - HKY18*P(3,9) + P(9,20));
-Kfusion(10) = HKY27*(HKY12*P(10,18) + HKY13*P(2,10) + HKY14*P(0,10) - HKY15*P(10,16) + HKY16*P(1,10) - HKY17*P(10,17) - HKY18*P(3,10) + P(10,20));
-Kfusion(11) = HKY27*(HKY12*P(11,18) + HKY13*P(2,11) + HKY14*P(0,11) - HKY15*P(11,16) + HKY16*P(1,11) - HKY17*P(11,17) - HKY18*P(3,11) + P(11,20));
-Kfusion(12) = HKY27*(HKY12*P(12,18) + HKY13*P(2,12) + HKY14*P(0,12) - HKY15*P(12,16) + HKY16*P(1,12) - HKY17*P(12,17) - HKY18*P(3,12) + P(12,20));
-Kfusion(13) = HKY27*(HKY12*P(13,18) + HKY13*P(2,13) + HKY14*P(0,13) - HKY15*P(13,16) + HKY16*P(1,13) - HKY17*P(13,17) - HKY18*P(3,13) + P(13,20));
-Kfusion(14) = HKY27*(HKY12*P(14,18) + HKY13*P(2,14) + HKY14*P(0,14) - HKY15*P(14,16) + HKY16*P(1,14) - HKY17*P(14,17) - HKY18*P(3,14) + P(14,20));
-Kfusion(15) = HKY27*(HKY12*P(15,18) + HKY13*P(2,15) + HKY14*P(0,15) - HKY15*P(15,16) + HKY16*P(1,15) - HKY17*P(15,17) - HKY18*P(3,15) + P(15,20));
-Kfusion(16) = HKY21*HKY27;
-Kfusion(17) = HKY20*HKY27;
-Kfusion(18) = HKY24*HKY27;
-Kfusion(19) = HKY27*(HKY12*P(18,19) + HKY13*P(2,19) + HKY14*P(0,19) - HKY15*P(16,19) + HKY16*P(1,19) - HKY17*P(17,19) - HKY18*P(3,19) + P(19,20));
-Kfusion(20) = HKY26*HKY27;
-Kfusion(21) = HKY27*(HKY12*P(18,21) + HKY13*P(2,21) + HKY14*P(0,21) - HKY15*P(16,21) + HKY16*P(1,21) - HKY17*P(17,21) - HKY18*P(3,21) + P(20,21));
-Kfusion(22) = HKY27*(HKY12*P(18,22) + HKY13*P(2,22) + HKY14*P(0,22) - HKY15*P(16,22) + HKY16*P(1,22) - HKY17*P(17,22) - HKY18*P(3,22) + P(20,22));
-Kfusion(23) = HKY27*(HKY12*P(18,23) + HKY13*P(2,23) + HKY14*P(0,23) - HKY15*P(16,23) + HKY16*P(1,23) - HKY17*P(17,23) - HKY18*P(3,23) + P(20,23));
+Kfusion(0) = HKY16*HKY24;
+Kfusion(1) = HKY21*HKY24;
+Kfusion(2) = HKY22*HKY24;
+Kfusion(3) = HKY19*HKY24;
+Kfusion(4) = HKY24*(HKY10*P(4,18) - HKY11*P(4,16) + HKY12*P(2,4) + HKY13*P(0,4) + HKY14*P(1,4) - HKY15*P(3,4) + HKY8*P(4,17) + P(4,20));
+Kfusion(5) = HKY24*(HKY10*P(5,18) - HKY11*P(5,16) + HKY12*P(2,5) + HKY13*P(0,5) + HKY14*P(1,5) - HKY15*P(3,5) + HKY8*P(5,17) + P(5,20));
+Kfusion(6) = HKY24*(HKY10*P(6,18) - HKY11*P(6,16) + HKY12*P(2,6) + HKY13*P(0,6) + HKY14*P(1,6) - HKY15*P(3,6) + HKY8*P(6,17) + P(6,20));
+Kfusion(7) = HKY24*(HKY10*P(7,18) - HKY11*P(7,16) + HKY12*P(2,7) + HKY13*P(0,7) + HKY14*P(1,7) - HKY15*P(3,7) + HKY8*P(7,17) + P(7,20));
+Kfusion(8) = HKY24*(HKY10*P(8,18) - HKY11*P(8,16) + HKY12*P(2,8) + HKY13*P(0,8) + HKY14*P(1,8) - HKY15*P(3,8) + HKY8*P(8,17) + P(8,20));
+Kfusion(9) = HKY24*(HKY10*P(9,18) - HKY11*P(9,16) + HKY12*P(2,9) + HKY13*P(0,9) + HKY14*P(1,9) - HKY15*P(3,9) + HKY8*P(9,17) + P(9,20));
+Kfusion(10) = HKY24*(HKY10*P(10,18) - HKY11*P(10,16) + HKY12*P(2,10) + HKY13*P(0,10) + HKY14*P(1,10) - HKY15*P(3,10) + HKY8*P(10,17) + P(10,20));
+Kfusion(11) = HKY24*(HKY10*P(11,18) - HKY11*P(11,16) + HKY12*P(2,11) + HKY13*P(0,11) + HKY14*P(1,11) - HKY15*P(3,11) + HKY8*P(11,17) + P(11,20));
+Kfusion(12) = HKY24*(HKY10*P(12,18) - HKY11*P(12,16) + HKY12*P(2,12) + HKY13*P(0,12) + HKY14*P(1,12) - HKY15*P(3,12) + HKY8*P(12,17) + P(12,20));
+Kfusion(13) = HKY24*(HKY10*P(13,18) - HKY11*P(13,16) + HKY12*P(2,13) + HKY13*P(0,13) + HKY14*P(1,13) - HKY15*P(3,13) + HKY8*P(13,17) + P(13,20));
+Kfusion(14) = HKY24*(HKY10*P(14,18) - HKY11*P(14,16) + HKY12*P(2,14) + HKY13*P(0,14) + HKY14*P(1,14) - HKY15*P(3,14) + HKY8*P(14,17) + P(14,20));
+Kfusion(15) = HKY24*(HKY10*P(15,18) - HKY11*P(15,16) + HKY12*P(2,15) + HKY13*P(0,15) + HKY14*P(1,15) - HKY15*P(3,15) + HKY8*P(15,17) + P(15,20));
+Kfusion(16) = HKY18*HKY24;
+Kfusion(17) = HKY17*HKY24;
+Kfusion(18) = HKY20*HKY24;
+Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
+Kfusion(20) = HKY23*HKY24;
+Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
+Kfusion(22) = HKY24*(HKY10*P(18,22) - HKY11*P(16,22) + HKY12*P(2,22) + HKY13*P(0,22) + HKY14*P(1,22) - HKY15*P(3,22) + HKY8*P(17,22) + P(20,22));
+Kfusion(23) = HKY24*(HKY10*P(18,23) - HKY11*P(16,23) + HKY12*P(2,23) + HKY13*P(0,23) + HKY14*P(1,23) - HKY15*P(3,23) + HKY8*P(17,23) + P(20,23));
+Kfusion(24) = HKY24*(HKY10*P(18,24) - HKY11*P(16,24) + HKY12*P(2,24) + HKY13*P(0,24) + HKY14*P(1,24) - HKY15*P(3,24) + HKY8*P(17,24) + P(20,24));
 
 
 // Axis 2 equations
 // Sub Expressions
-const float HKZ0 = magN*q2;
-const float HKZ1 = magE*q1;
-const float HKZ2 = magN*q3;
-const float HKZ3 = 2*magD;
-const float HKZ4 = HKZ3*q1;
-const float HKZ5 = magE*q0;
-const float HKZ6 = -HKZ3*q2 + magE*q3 + magN*q0;
-const float HKZ7 = magE*q2 + magN*q1;
-const float HKZ8 = q0*q2 + q1*q3;
-const float HKZ9 = q2*q3;
-const float HKZ10 = q0*q1;
-const float HKZ11 = 2*powf(q1, 2);
-const float HKZ12 = 2*powf(q2, 2);
-const float HKZ13 = 2*HKZ8;
-const float HKZ14 = HKZ13*P(0,16);
-const float HKZ15 = 2*HKZ7;
-const float HKZ16 = HKZ15*P(0,3);
-const float HKZ17 = -2*HKZ0 + 2*HKZ1;
-const float HKZ18 = HKZ17*P(0,0);
-const float HKZ19 = 2*HKZ10 - 2*HKZ9;
-const float HKZ20 = HKZ19*P(0,17);
-const float HKZ21 = HKZ11 + HKZ12 - 1;
-const float HKZ22 = HKZ21*P(0,18);
-const float HKZ23 = 2*HKZ6;
-const float HKZ24 = HKZ23*P(0,2);
-const float HKZ25 = -2*HKZ2 + 2*HKZ4 + 2*HKZ5;
-const float HKZ26 = HKZ25*P(0,1);
-const float HKZ27 = HKZ13*P(16,18);
-const float HKZ28 = HKZ15*P(3,18);
-const float HKZ29 = HKZ17*P(0,18);
-const float HKZ30 = HKZ19*P(17,18);
-const float HKZ31 = HKZ23*P(2,18);
-const float HKZ32 = HKZ25*P(1,18);
-const float HKZ33 = HKZ21*P(18,18);
-const float HKZ34 = HKZ13*P(3,16);
-const float HKZ35 = HKZ15*P(3,3);
-const float HKZ36 = HKZ17*P(0,3);
-const float HKZ37 = HKZ19*P(3,17);
-const float HKZ38 = HKZ23*P(2,3);
-const float HKZ39 = HKZ25*P(1,3);
-const float HKZ40 = HKZ21*P(3,18);
-const float HKZ41 = HKZ13*P(16,16);
-const float HKZ42 = HKZ15*P(3,16);
-const float HKZ43 = HKZ17*P(0,16);
-const float HKZ44 = HKZ19*P(16,17);
-const float HKZ45 = HKZ23*P(2,16);
-const float HKZ46 = HKZ25*P(1,16);
-const float HKZ47 = HKZ21*P(16,18);
-const float HKZ48 = HKZ13*P(2,16);
-const float HKZ49 = HKZ15*P(2,3);
-const float HKZ50 = HKZ17*P(0,2);
-const float HKZ51 = HKZ19*P(2,17);
-const float HKZ52 = HKZ23*P(2,2);
-const float HKZ53 = HKZ25*P(1,2);
-const float HKZ54 = HKZ21*P(2,18);
-const float HKZ55 = HKZ13*P(16,17);
-const float HKZ56 = HKZ15*P(3,17);
-const float HKZ57 = HKZ17*P(0,17);
-const float HKZ58 = HKZ19*P(17,17);
-const float HKZ59 = HKZ23*P(2,17);
-const float HKZ60 = HKZ25*P(1,17);
-const float HKZ61 = HKZ21*P(17,18);
-const float HKZ62 = HKZ13*P(1,16);
-const float HKZ63 = HKZ15*P(1,3);
-const float HKZ64 = HKZ17*P(0,1);
-const float HKZ65 = HKZ19*P(1,17);
-const float HKZ66 = HKZ23*P(1,2);
-const float HKZ67 = HKZ25*P(1,1);
-const float HKZ68 = HKZ21*P(1,18);
-const float HKZ69 = -HKZ13*P(16,21) - HKZ15*P(3,21) + HKZ17*P(0,21) + HKZ19*P(17,21) + HKZ21*P(18,21) - HKZ23*P(2,21) + HKZ25*P(1,21) - P(21,21);
-const float HKZ70 = 1.0F/(-HKZ13*(HKZ41 + HKZ42 - HKZ43 - HKZ44 + HKZ45 - HKZ46 - HKZ47 + P(16,21)) - HKZ15*(HKZ34 + HKZ35 - HKZ36 - HKZ37 + HKZ38 - HKZ39 - HKZ40 + P(3,21)) + HKZ17*(HKZ14 + HKZ16 - HKZ18 - HKZ20 - HKZ22 + HKZ24 - HKZ26 + P(0,21)) + HKZ19*(HKZ55 + HKZ56 - HKZ57 - HKZ58 + HKZ59 - HKZ60 - HKZ61 + P(17,21)) + HKZ21*(HKZ27 + HKZ28 - HKZ29 - HKZ30 + HKZ31 - HKZ32 - HKZ33 + P(18,21)) - HKZ23*(HKZ48 + HKZ49 - HKZ50 - HKZ51 + HKZ52 - HKZ53 - HKZ54 + P(2,21)) + HKZ25*(HKZ62 + HKZ63 - HKZ64 - HKZ65 + HKZ66 - HKZ67 - HKZ68 + P(1,21)) + HKZ69 - R_MAG);
+const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
+const float HKZ1 = magN*q3;
+const float HKZ2 = magD*q1;
+const float HKZ3 = magE*q0;
+const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
+const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
+const float HKZ6 = q0*q2 + q1*q3;
+const float HKZ7 = q2*q3;
+const float HKZ8 = q0*q1;
+const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
+const float HKZ10 = 2*HKZ6;
+const float HKZ11 = -2*HKZ7 + 2*HKZ8;
+const float HKZ12 = 2*HKZ5;
+const float HKZ13 = 2*HKZ0;
+const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
+const float HKZ15 = 2*HKZ4;
+const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
+const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
+const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
+const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
+const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
+const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
+const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
+const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
+const float HKZ24 = 1.0F/(HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = 2*HKZ0 - 2*HKZ1;
-Hfusion.at<1>() = 2*HKZ2 - 2*HKZ4 - 2*HKZ5;
-Hfusion.at<2>() = 2*HKZ6;
-Hfusion.at<3>() = 2*HKZ7;
+Hfusion.at<0>() = 2*HKZ0;
+Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
+Hfusion.at<2>() = 2*HKZ4;
+Hfusion.at<3>() = 2*HKZ5;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -305,38 +214,40 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = 2*HKZ8;
-Hfusion.at<17>() = -2*HKZ10 + 2*HKZ9;
-Hfusion.at<18>() = -HKZ11 - HKZ12 + 1;
+Hfusion.at<16>() = 2*HKZ6;
+Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
+Hfusion.at<18>() = HKZ9;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 1;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HKZ70*(-HKZ14 - HKZ16 + HKZ18 + HKZ20 + HKZ22 - HKZ24 + HKZ26 - P(0,21));
-Kfusion(1) = HKZ70*(-HKZ62 - HKZ63 + HKZ64 + HKZ65 - HKZ66 + HKZ67 + HKZ68 - P(1,21));
-Kfusion(2) = HKZ70*(-HKZ48 - HKZ49 + HKZ50 + HKZ51 - HKZ52 + HKZ53 + HKZ54 - P(2,21));
-Kfusion(3) = HKZ70*(-HKZ34 - HKZ35 + HKZ36 + HKZ37 - HKZ38 + HKZ39 + HKZ40 - P(3,21));
-Kfusion(4) = HKZ70*(-HKZ13*P(4,16) - HKZ15*P(3,4) + HKZ17*P(0,4) + HKZ19*P(4,17) + HKZ21*P(4,18) - HKZ23*P(2,4) + HKZ25*P(1,4) - P(4,21));
-Kfusion(5) = HKZ70*(-HKZ13*P(5,16) - HKZ15*P(3,5) + HKZ17*P(0,5) + HKZ19*P(5,17) + HKZ21*P(5,18) - HKZ23*P(2,5) + HKZ25*P(1,5) - P(5,21));
-Kfusion(6) = HKZ70*(-HKZ13*P(6,16) - HKZ15*P(3,6) + HKZ17*P(0,6) + HKZ19*P(6,17) + HKZ21*P(6,18) - HKZ23*P(2,6) + HKZ25*P(1,6) - P(6,21));
-Kfusion(7) = HKZ70*(-HKZ13*P(7,16) - HKZ15*P(3,7) + HKZ17*P(0,7) + HKZ19*P(7,17) + HKZ21*P(7,18) - HKZ23*P(2,7) + HKZ25*P(1,7) - P(7,21));
-Kfusion(8) = HKZ70*(-HKZ13*P(8,16) - HKZ15*P(3,8) + HKZ17*P(0,8) + HKZ19*P(8,17) + HKZ21*P(8,18) - HKZ23*P(2,8) + HKZ25*P(1,8) - P(8,21));
-Kfusion(9) = HKZ70*(-HKZ13*P(9,16) - HKZ15*P(3,9) + HKZ17*P(0,9) + HKZ19*P(9,17) + HKZ21*P(9,18) - HKZ23*P(2,9) + HKZ25*P(1,9) - P(9,21));
-Kfusion(10) = HKZ70*(-HKZ13*P(10,16) - HKZ15*P(3,10) + HKZ17*P(0,10) + HKZ19*P(10,17) + HKZ21*P(10,18) - HKZ23*P(2,10) + HKZ25*P(1,10) - P(10,21));
-Kfusion(11) = HKZ70*(-HKZ13*P(11,16) - HKZ15*P(3,11) + HKZ17*P(0,11) + HKZ19*P(11,17) + HKZ21*P(11,18) - HKZ23*P(2,11) + HKZ25*P(1,11) - P(11,21));
-Kfusion(12) = HKZ70*(-HKZ13*P(12,16) - HKZ15*P(3,12) + HKZ17*P(0,12) + HKZ19*P(12,17) + HKZ21*P(12,18) - HKZ23*P(2,12) + HKZ25*P(1,12) - P(12,21));
-Kfusion(13) = HKZ70*(-HKZ13*P(13,16) - HKZ15*P(3,13) + HKZ17*P(0,13) + HKZ19*P(13,17) + HKZ21*P(13,18) - HKZ23*P(2,13) + HKZ25*P(1,13) - P(13,21));
-Kfusion(14) = HKZ70*(-HKZ13*P(14,16) - HKZ15*P(3,14) + HKZ17*P(0,14) + HKZ19*P(14,17) + HKZ21*P(14,18) - HKZ23*P(2,14) + HKZ25*P(1,14) - P(14,21));
-Kfusion(15) = HKZ70*(-HKZ13*P(15,16) - HKZ15*P(3,15) + HKZ17*P(0,15) + HKZ19*P(15,17) + HKZ21*P(15,18) - HKZ23*P(2,15) + HKZ25*P(1,15) - P(15,21));
-Kfusion(16) = HKZ70*(-HKZ41 - HKZ42 + HKZ43 + HKZ44 - HKZ45 + HKZ46 + HKZ47 - P(16,21));
-Kfusion(17) = HKZ70*(-HKZ55 - HKZ56 + HKZ57 + HKZ58 - HKZ59 + HKZ60 + HKZ61 - P(17,21));
-Kfusion(18) = HKZ70*(-HKZ27 - HKZ28 + HKZ29 + HKZ30 - HKZ31 + HKZ32 + HKZ33 - P(18,21));
-Kfusion(19) = HKZ70*(-HKZ13*P(16,19) - HKZ15*P(3,19) + HKZ17*P(0,19) + HKZ19*P(17,19) + HKZ21*P(18,19) - HKZ23*P(2,19) + HKZ25*P(1,19) - P(19,21));
-Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(17,20) + HKZ21*P(18,20) - HKZ23*P(2,20) + HKZ25*P(1,20) - P(20,21));
-Kfusion(21) = HKZ69*HKZ70;
-Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22));
-Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23));
+Kfusion(0) = HKZ16*HKZ24;
+Kfusion(1) = HKZ19*HKZ24;
+Kfusion(2) = HKZ22*HKZ24;
+Kfusion(3) = HKZ21*HKZ24;
+Kfusion(4) = HKZ24*(HKZ10*P(4,16) - HKZ11*P(4,17) + HKZ12*P(3,4) + HKZ13*P(0,4) - HKZ14*P(1,4) + HKZ15*P(2,4) + HKZ9*P(4,18) + P(4,21));
+Kfusion(5) = HKZ24*(HKZ10*P(5,16) - HKZ11*P(5,17) + HKZ12*P(3,5) + HKZ13*P(0,5) - HKZ14*P(1,5) + HKZ15*P(2,5) + HKZ9*P(5,18) + P(5,21));
+Kfusion(6) = HKZ24*(HKZ10*P(6,16) - HKZ11*P(6,17) + HKZ12*P(3,6) + HKZ13*P(0,6) - HKZ14*P(1,6) + HKZ15*P(2,6) + HKZ9*P(6,18) + P(6,21));
+Kfusion(7) = HKZ24*(HKZ10*P(7,16) - HKZ11*P(7,17) + HKZ12*P(3,7) + HKZ13*P(0,7) - HKZ14*P(1,7) + HKZ15*P(2,7) + HKZ9*P(7,18) + P(7,21));
+Kfusion(8) = HKZ24*(HKZ10*P(8,16) - HKZ11*P(8,17) + HKZ12*P(3,8) + HKZ13*P(0,8) - HKZ14*P(1,8) + HKZ15*P(2,8) + HKZ9*P(8,18) + P(8,21));
+Kfusion(9) = HKZ24*(HKZ10*P(9,16) - HKZ11*P(9,17) + HKZ12*P(3,9) + HKZ13*P(0,9) - HKZ14*P(1,9) + HKZ15*P(2,9) + HKZ9*P(9,18) + P(9,21));
+Kfusion(10) = HKZ24*(HKZ10*P(10,16) - HKZ11*P(10,17) + HKZ12*P(3,10) + HKZ13*P(0,10) - HKZ14*P(1,10) + HKZ15*P(2,10) + HKZ9*P(10,18) + P(10,21));
+Kfusion(11) = HKZ24*(HKZ10*P(11,16) - HKZ11*P(11,17) + HKZ12*P(3,11) + HKZ13*P(0,11) - HKZ14*P(1,11) + HKZ15*P(2,11) + HKZ9*P(11,18) + P(11,21));
+Kfusion(12) = HKZ24*(HKZ10*P(12,16) - HKZ11*P(12,17) + HKZ12*P(3,12) + HKZ13*P(0,12) - HKZ14*P(1,12) + HKZ15*P(2,12) + HKZ9*P(12,18) + P(12,21));
+Kfusion(13) = HKZ24*(HKZ10*P(13,16) - HKZ11*P(13,17) + HKZ12*P(3,13) + HKZ13*P(0,13) - HKZ14*P(1,13) + HKZ15*P(2,13) + HKZ9*P(13,18) + P(13,21));
+Kfusion(14) = HKZ24*(HKZ10*P(14,16) - HKZ11*P(14,17) + HKZ12*P(3,14) + HKZ13*P(0,14) - HKZ14*P(1,14) + HKZ15*P(2,14) + HKZ9*P(14,18) + P(14,21));
+Kfusion(15) = HKZ24*(HKZ10*P(15,16) - HKZ11*P(15,17) + HKZ12*P(3,15) + HKZ13*P(0,15) - HKZ14*P(1,15) + HKZ15*P(2,15) + HKZ9*P(15,18) + P(15,21));
+Kfusion(16) = HKZ20*HKZ24;
+Kfusion(17) = HKZ18*HKZ24;
+Kfusion(18) = HKZ17*HKZ24;
+Kfusion(19) = HKZ24*(HKZ10*P(16,19) - HKZ11*P(17,19) + HKZ12*P(3,19) + HKZ13*P(0,19) - HKZ14*P(1,19) + HKZ15*P(2,19) + HKZ9*P(18,19) + P(19,21));
+Kfusion(20) = HKZ24*(HKZ10*P(16,20) - HKZ11*P(17,20) + HKZ12*P(3,20) + HKZ13*P(0,20) - HKZ14*P(1,20) + HKZ15*P(2,20) + HKZ9*P(18,20) + P(20,21));
+Kfusion(21) = HKZ23*HKZ24;
+Kfusion(22) = HKZ24*(HKZ10*P(16,22) - HKZ11*P(17,22) + HKZ12*P(3,22) + HKZ13*P(0,22) - HKZ14*P(1,22) + HKZ15*P(2,22) + HKZ9*P(18,22) + P(21,22));
+Kfusion(23) = HKZ24*(HKZ10*P(16,23) - HKZ11*P(17,23) + HKZ12*P(3,23) + HKZ13*P(0,23) - HKZ14*P(1,23) + HKZ15*P(2,23) + HKZ9*P(18,23) + P(21,23));
+Kfusion(24) = HKZ24*(HKZ10*P(16,24) - HKZ11*P(17,24) + HKZ12*P(3,24) + HKZ13*P(0,24) - HKZ14*P(1,24) + HKZ15*P(2,24) + HKZ9*P(18,24) + P(21,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp

@@ -1,186 +1,96 @@
 // Sub Expressions
 const float HK0 = magE*q3;
-const float HK1 = magD*q2;
-const float HK2 = -HK1;
-const float HK3 = magD*q3;
-const float HK4 = magE*q2;
-const float HK5 = HK3 + HK4;
-const float HK6 = magE*q1;
-const float HK7 = magD*q0;
-const float HK8 = magN*q2;
-const float HK9 = 2*HK8;
+const float HK1 = magN*q0;
+const float HK2 = magD*q2;
+const float HK3 = HK0 + HK1 - HK2;
+const float HK4 = 2*HK3;
+const float HK5 = magD*q3 + magE*q2 + magN*q1;
+const float HK6 = 2*HK5;
+const float HK7 = magE*q1;
+const float HK8 = magD*q0;
+const float HK9 = magN*q2;
 const float HK10 = magD*q1;
 const float HK11 = magE*q0;
 const float HK12 = magN*q3;
-const float HK13 = HK10 + HK11 - 2*HK12;
-const float HK14 = 2*powf(q2, 2);
-const float HK15 = -HK14;
-const float HK16 = 2*powf(q3, 2);
-const float HK17 = 1 - HK16;
-const float HK18 = q0*q3;
-const float HK19 = q1*q2;
-const float HK20 = HK18 + HK19;
-const float HK21 = q1*q3;
-const float HK22 = q0*q2;
-const float HK23 = 2*HK5;
-const float HK24 = HK23*P(0,1);
-const float HK25 = 2*HK20;
-const float HK26 = HK25*P(0,17);
-const float HK27 = -2*HK0 + 2*HK1;
-const float HK28 = HK27*P(0,0);
-const float HK29 = -2*HK21 + 2*HK22;
-const float HK30 = HK29*P(0,18);
-const float HK31 = HK16 - 1;
-const float HK32 = HK14 + HK31;
-const float HK33 = HK32*P(0,16);
+const float HK13 = HK10 + HK11 - HK12;
+const float HK14 = 2*HK13;
+const float HK15 = powf(q1, 2);
+const float HK16 = powf(q2, 2);
+const float HK17 = -HK16;
+const float HK18 = powf(q0, 2);
+const float HK19 = powf(q3, 2);
+const float HK20 = HK18 - HK19;
+const float HK21 = HK15 + HK17 + HK20;
+const float HK22 = q0*q3;
+const float HK23 = q1*q2;
+const float HK24 = HK22 + HK23;
+const float HK25 = q1*q3;
+const float HK26 = q0*q2;
+const float HK27 = 2*HK24;
+const float HK28 = -2*HK25 + 2*HK26;
+const float HK29 = 2*HK5;
+const float HK30 = 2*HK3;
+const float HK31 = -HK7 + HK8 + HK9;
+const float HK32 = 2*HK31;
+const float HK33 = HK32*P(0,2);
 const float HK34 = 2*HK13;
 const float HK35 = HK34*P(0,3);
-const float HK36 = -HK6;
-const float HK37 = 2*HK36 + 2*HK7 + 2*HK9;
-const float HK38 = HK37*P(0,2);
-const float HK39 = -R_MAG;
-const float HK40 = HK23*P(1,16);
-const float HK41 = HK25*P(16,17);
-const float HK42 = HK27*P(0,16);
-const float HK43 = HK29*P(16,18);
-const float HK44 = HK34*P(3,16);
-const float HK45 = HK32*P(16,16);
-const float HK46 = HK37*P(2,16);
-const float HK47 = HK23*P(1,1);
-const float HK48 = HK25*P(1,17);
-const float HK49 = HK27*P(0,1);
-const float HK50 = HK29*P(1,18);
-const float HK51 = HK34*P(1,3);
-const float HK52 = HK32*P(1,16);
-const float HK53 = HK37*P(1,2);
-const float HK54 = HK23*P(1,17);
-const float HK55 = HK25*P(17,17);
-const float HK56 = HK27*P(0,17);
-const float HK57 = HK29*P(17,18);
-const float HK58 = HK34*P(3,17);
-const float HK59 = HK32*P(16,17);
-const float HK60 = HK37*P(2,17);
-const float HK61 = HK23*P(1,3);
-const float HK62 = HK25*P(3,17);
-const float HK63 = HK27*P(0,3);
-const float HK64 = HK29*P(3,18);
-const float HK65 = HK34*P(3,3);
-const float HK66 = HK37*P(2,3);
-const float HK67 = HK32*P(3,16);
-const float HK68 = HK23*P(1,18);
-const float HK69 = HK25*P(17,18);
-const float HK70 = HK27*P(0,18);
-const float HK71 = HK29*P(18,18);
-const float HK72 = HK34*P(3,18);
-const float HK73 = HK32*P(16,18);
-const float HK74 = HK37*P(2,18);
-const float HK75 = HK23*P(1,2);
-const float HK76 = HK25*P(2,17);
-const float HK77 = HK27*P(0,2);
-const float HK78 = HK29*P(2,18);
-const float HK79 = HK34*P(2,3);
-const float HK80 = HK32*P(2,16);
-const float HK81 = HK37*P(2,2);
-const float HK82 = -HK23*P(1,19) - HK25*P(17,19) + HK27*P(0,19) + HK29*P(18,19) + HK32*P(16,19) - HK34*P(3,19) + HK37*P(2,19) - P(19,19);
-const float HK83 = 1.0F/(-HK23*(HK47 + HK48 - HK49 - HK50 + HK51 - HK52 - HK53 + P(1,19)) - HK25*(HK54 + HK55 - HK56 - HK57 + HK58 - HK59 - HK60 + P(17,19)) + HK27*(HK24 + HK26 - HK28 - HK30 - HK33 + HK35 - HK38 + P(0,19)) + HK29*(HK68 + HK69 - HK70 - HK71 + HK72 - HK73 - HK74 + P(18,19)) + HK32*(HK40 + HK41 - HK42 - HK43 + HK44 - HK45 - HK46 + P(16,19)) - HK34*(HK61 + HK62 - HK63 - HK64 + HK65 - HK66 - HK67 + P(3,19)) + HK37*(HK75 + HK76 - HK77 - HK78 + HK79 - HK80 - HK81 + P(2,19)) + HK39 + HK82);
-const float HK84 = -P(19,21);
-const float HK85 = -HK12;
-const float HK86 = HK10 + HK85;
-const float HK87 = -2*HK6 + HK7 + HK8;
-const float HK88 = magN*q1;
-const float HK89 = HK3 + HK88;
-const float HK90 = 2*HK0;
-const float HK91 = magN*q0;
-const float HK92 = 2*powf(q1, 2);
-const float HK93 = -HK92;
-const float HK94 = q0*q1;
-const float HK95 = q2*q3;
-const float HK96 = HK94 + HK95;
-const float HK97 = 2*HK96;
-const float HK98 = 2*HK89;
-const float HK99 = 2*HK86;
-const float HK100 = 2*HK18 - 2*HK19;
-const float HK101 = 2*HK87;
-const float HK102 = HK31 + HK92;
-const float HK103 = 2*HK2 + 2*HK90 + 2*HK91;
-const float HK104 = -HK100*P(0,16) + HK101*P(0,1) - HK102*P(0,17) - HK103*P(0,3) + HK97*P(0,18) + HK98*P(0,2) + HK99*P(0,0) + P(0,20);
-const float HK105 = -HK100*P(16,17) + HK101*P(1,17) - HK102*P(17,17) - HK103*P(3,17) + HK97*P(17,18) + HK98*P(2,17) + HK99*P(0,17) + P(17,20);
-const float HK106 = -HK100*P(16,16) + HK101*P(1,16) - HK102*P(16,17) - HK103*P(3,16) + HK97*P(16,18) + HK98*P(2,16) + HK99*P(0,16) + P(16,20);
-const float HK107 = -HK100*P(3,16) + HK101*P(1,3) - HK102*P(3,17) - HK103*P(3,3) + HK97*P(3,18) + HK98*P(2,3) + HK99*P(0,3) + P(3,20);
-const float HK108 = -HK100*P(2,16) + HK101*P(1,2) - HK102*P(2,17) - HK103*P(2,3) + HK97*P(2,18) + HK98*P(2,2) + HK99*P(0,2) + P(2,20);
-const float HK109 = -HK100*P(16,18) + HK101*P(1,18) - HK102*P(17,18) - HK103*P(3,18) + HK97*P(18,18) + HK98*P(2,18) + HK99*P(0,18) + P(18,20);
-const float HK110 = -HK100*P(1,16) + HK101*P(1,1) - HK102*P(1,17) - HK103*P(1,3) + HK97*P(1,18) + HK98*P(1,2) + HK99*P(0,1) + P(1,20);
-const float HK111 = -HK100*P(16,20) + HK101*P(1,20) - HK102*P(17,20) - HK103*P(3,20) + HK97*P(18,20) + HK98*P(2,20) + HK99*P(0,20) + P(20,20);
-const float HK112 = 1.0F/(-HK100*HK106 + HK101*HK110 - HK102*HK105 - HK103*HK107 + HK104*HK99 + HK108*HK98 + HK109*HK97 + HK111 + R_MAG);
-const float HK113 = 2*HK10;
-const float HK114 = HK0 - 2*HK1 + HK91;
-const float HK115 = HK4 + HK88;
-const float HK116 = HK21 + HK22;
-const float HK117 = 2*HK116;
-const float HK118 = HK117*P(0,16);
-const float HK119 = 2*HK115;
-const float HK120 = HK119*P(0,3);
-const float HK121 = 2*HK6 - 2*HK8;
-const float HK122 = HK121*P(0,0);
-const float HK123 = 2*HK94 - 2*HK95;
-const float HK124 = HK123*P(0,17);
-const float HK125 = HK14 + HK92 - 1;
-const float HK126 = HK125*P(0,18);
-const float HK127 = 2*HK114;
-const float HK128 = HK127*P(0,2);
-const float HK129 = 2*HK11 + 2*HK113 + 2*HK85;
-const float HK130 = HK129*P(0,1);
-const float HK131 = HK117*P(16,18);
-const float HK132 = HK119*P(3,18);
-const float HK133 = HK121*P(0,18);
-const float HK134 = HK123*P(17,18);
-const float HK135 = HK127*P(2,18);
-const float HK136 = HK129*P(1,18);
-const float HK137 = HK125*P(18,18);
-const float HK138 = HK117*P(3,16);
-const float HK139 = HK119*P(3,3);
-const float HK140 = HK121*P(0,3);
-const float HK141 = HK123*P(3,17);
-const float HK142 = HK127*P(2,3);
-const float HK143 = HK129*P(1,3);
-const float HK144 = HK125*P(3,18);
-const float HK145 = HK117*P(16,16);
-const float HK146 = HK119*P(3,16);
-const float HK147 = HK121*P(0,16);
-const float HK148 = HK123*P(16,17);
-const float HK149 = HK127*P(2,16);
-const float HK150 = HK129*P(1,16);
-const float HK151 = HK125*P(16,18);
-const float HK152 = HK117*P(2,16);
-const float HK153 = HK119*P(2,3);
-const float HK154 = HK121*P(0,2);
-const float HK155 = HK123*P(2,17);
-const float HK156 = HK127*P(2,2);
-const float HK157 = HK129*P(1,2);
-const float HK158 = HK125*P(2,18);
-const float HK159 = HK117*P(16,17);
-const float HK160 = HK119*P(3,17);
-const float HK161 = HK121*P(0,17);
-const float HK162 = HK123*P(17,17);
-const float HK163 = HK127*P(2,17);
-const float HK164 = HK129*P(1,17);
-const float HK165 = HK125*P(17,18);
-const float HK166 = HK117*P(1,16);
-const float HK167 = HK119*P(1,3);
-const float HK168 = HK121*P(0,1);
-const float HK169 = HK123*P(1,17);
-const float HK170 = HK127*P(1,2);
-const float HK171 = HK129*P(1,1);
-const float HK172 = HK125*P(1,18);
-const float HK173 = -HK117*P(16,21) - HK119*P(3,21) + HK121*P(0,21) + HK123*P(17,21) + HK125*P(18,21) - HK127*P(2,21) + HK129*P(1,21) - P(21,21);
-const float HK174 = 1.0F/(-HK117*(HK145 + HK146 - HK147 - HK148 + HK149 - HK150 - HK151 + P(16,21)) - HK119*(HK138 + HK139 - HK140 - HK141 + HK142 - HK143 - HK144 + P(3,21)) + HK121*(HK118 + HK120 - HK122 - HK124 - HK126 + HK128 - HK130 + P(0,21)) + HK123*(HK159 + HK160 - HK161 - HK162 + HK163 - HK164 - HK165 + P(17,21)) + HK125*(HK131 + HK132 - HK133 - HK134 + HK135 - HK136 - HK137 + P(18,21)) - HK127*(HK152 + HK153 - HK154 - HK155 + HK156 - HK157 - HK158 + P(2,21)) + HK129*(HK166 + HK167 - HK168 - HK169 + HK170 - HK171 - HK172 + P(1,21)) + HK173 + HK39);
+const float HK36 = HK21*P(0,16) + HK27*P(0,17) - HK28*P(0,18) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35 + P(0,19);
+const float HK37 = HK21*P(16,16) + HK27*P(16,17) - HK28*P(16,18) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16) + P(16,19);
+const float HK38 = HK21*P(16,18) + HK27*P(17,18) - HK28*P(18,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18) + P(18,19);
+const float HK39 = HK29*P(1,2);
+const float HK40 = HK30*P(0,2);
+const float HK41 = HK21*P(2,16) + HK27*P(2,17) - HK28*P(2,18) - HK32*P(2,2) + HK34*P(2,3) + HK39 + HK40 + P(2,19);
+const float HK42 = HK21*P(16,17) + HK27*P(17,17) - HK28*P(17,18) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17) + P(17,19);
+const float HK43 = HK29*P(1,3);
+const float HK44 = HK30*P(0,3);
+const float HK45 = HK21*P(3,16) + HK27*P(3,17) - HK28*P(3,18) - HK32*P(2,3) + HK34*P(3,3) + HK43 + HK44 + P(3,19);
+const float HK46 = HK32*P(1,2);
+const float HK47 = HK34*P(1,3);
+const float HK48 = HK21*P(1,16) + HK27*P(1,17) - HK28*P(1,18) + HK29*P(1,1) + HK30*P(0,1) - HK46 + HK47 + P(1,19);
+const float HK49 = HK21*P(16,19) + HK27*P(17,19) - HK28*P(18,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19) + P(19,19);
+const float HK50 = 1.0F/(HK21*HK37 + HK27*HK42 - HK28*HK38 + HK29*HK48 + HK30*HK36 - HK32*HK41 + HK34*HK45 + HK49 + R_MAG);
+const float HK51 = 2*HK31;
+const float HK52 = -HK15;
+const float HK53 = HK16 + HK20 + HK52;
+const float HK54 = q0*q1;
+const float HK55 = q2*q3;
+const float HK56 = HK54 + HK55;
+const float HK57 = 2*HK56;
+const float HK58 = 2*HK22 - 2*HK23;
+const float HK59 = HK32*P(0,1);
+const float HK60 = HK29*P(0,2) + HK34*P(0,0) - HK44 + HK53*P(0,17) + HK57*P(0,18) - HK58*P(0,16) + HK59 + P(0,20);
+const float HK61 = HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK53*P(17,17) + HK57*P(17,18) - HK58*P(16,17) + P(17,20);
+const float HK62 = HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK53*P(16,17) + HK57*P(16,18) - HK58*P(16,16) + P(16,20);
+const float HK63 = HK29*P(2,3);
+const float HK64 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK53*P(3,17) + HK57*P(3,18) - HK58*P(3,16) + HK63 + P(3,20);
+const float HK65 = HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK53*P(17,18) + HK57*P(18,18) - HK58*P(16,18) + P(18,20);
+const float HK66 = HK34*P(0,1);
+const float HK67 = -HK30*P(1,3) + HK32*P(1,1) + HK39 + HK53*P(1,17) + HK57*P(1,18) - HK58*P(1,16) + HK66 + P(1,20);
+const float HK68 = HK30*P(2,3);
+const float HK69 = HK29*P(2,2) + HK34*P(0,2) + HK46 + HK53*P(2,17) + HK57*P(2,18) - HK58*P(2,16) - HK68 + P(2,20);
+const float HK70 = HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK53*P(17,20) + HK57*P(18,20) - HK58*P(16,20) + P(20,20);
+const float HK71 = 1.0F/(HK29*HK69 - HK30*HK64 + HK32*HK67 + HK34*HK60 + HK53*HK61 + HK57*HK65 - HK58*HK62 + HK70 + R_MAG);
+const float HK72 = HK25 + HK26;
+const float HK73 = HK17 + HK18 + HK19 + HK52;
+const float HK74 = 2*HK72;
+const float HK75 = 2*HK54 - 2*HK55;
+const float HK76 = HK29*P(0,3) + HK32*P(0,0) + HK40 - HK66 + HK73*P(0,18) + HK74*P(0,16) - HK75*P(0,17) + P(0,21);
+const float HK77 = HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK73*P(18,18) + HK74*P(16,18) - HK75*P(17,18) + P(18,21);
+const float HK78 = HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK73*P(17,18) + HK74*P(16,17) - HK75*P(17,17) + P(17,21);
+const float HK79 = HK30*P(1,2) - HK34*P(1,1) + HK43 + HK59 + HK73*P(1,18) + HK74*P(1,16) - HK75*P(1,17) + P(1,21);
+const float HK80 = HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK73*P(16,18) + HK74*P(16,16) - HK75*P(16,17) + P(16,21);
+const float HK81 = HK29*P(3,3) + HK32*P(0,3) - HK47 + HK68 + HK73*P(3,18) + HK74*P(3,16) - HK75*P(3,17) + P(3,21);
+const float HK82 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK63 + HK73*P(2,18) + HK74*P(2,16) - HK75*P(2,17) + P(2,21);
+const float HK83 = HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK73*P(18,21) + HK74*P(16,21) - HK75*P(17,21) + P(21,21);
+const float HK84 = 1.0F/(HK29*HK81 + HK30*HK82 + HK32*HK76 - HK34*HK79 + HK73*HK77 + HK74*HK80 - HK75*HK78 + HK83 + R_MAG);
 
 
 // Observation Jacobians - axis 0
-Hfusion.at<0>() = 2*HK0 + 2*HK2;
-Hfusion.at<1>() = 2*HK5;
-Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9;
-Hfusion.at<3>() = 2*HK13;
+Hfusion.at<0>() = HK4;
+Hfusion.at<1>() = HK6;
+Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9;
+Hfusion.at<3>() = HK14;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -193,48 +103,50 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = HK15 + HK17;
-Hfusion.at<17>() = 2*HK20;
-Hfusion.at<18>() = 2*HK21 - 2*HK22;
+Hfusion.at<16>() = HK21;
+Hfusion.at<17>() = 2*HK24;
+Hfusion.at<18>() = 2*HK25 - 2*HK26;
 Hfusion.at<19>() = 1;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 0
-Kfusion(0) = HK83*(-HK24 - HK26 + HK28 + HK30 + HK33 - HK35 + HK38 - P(0,19));
-Kfusion(1) = HK83*(-HK47 - HK48 + HK49 + HK50 - HK51 + HK52 + HK53 - P(1,19));
-Kfusion(2) = HK83*(-HK75 - HK76 + HK77 + HK78 - HK79 + HK80 + HK81 - P(2,19));
-Kfusion(3) = HK83*(-HK61 - HK62 + HK63 + HK64 - HK65 + HK66 + HK67 - P(3,19));
-Kfusion(4) = HK83*(-HK23*P(1,4) - HK25*P(4,17) + HK27*P(0,4) + HK29*P(4,18) + HK32*P(4,16) - HK34*P(3,4) + HK37*P(2,4) - P(4,19));
-Kfusion(5) = HK83*(-HK23*P(1,5) - HK25*P(5,17) + HK27*P(0,5) + HK29*P(5,18) + HK32*P(5,16) - HK34*P(3,5) + HK37*P(2,5) - P(5,19));
-Kfusion(6) = HK83*(-HK23*P(1,6) - HK25*P(6,17) + HK27*P(0,6) + HK29*P(6,18) + HK32*P(6,16) - HK34*P(3,6) + HK37*P(2,6) - P(6,19));
-Kfusion(7) = HK83*(-HK23*P(1,7) - HK25*P(7,17) + HK27*P(0,7) + HK29*P(7,18) + HK32*P(7,16) - HK34*P(3,7) + HK37*P(2,7) - P(7,19));
-Kfusion(8) = HK83*(-HK23*P(1,8) - HK25*P(8,17) + HK27*P(0,8) + HK29*P(8,18) + HK32*P(8,16) - HK34*P(3,8) + HK37*P(2,8) - P(8,19));
-Kfusion(9) = HK83*(-HK23*P(1,9) - HK25*P(9,17) + HK27*P(0,9) + HK29*P(9,18) + HK32*P(9,16) - HK34*P(3,9) + HK37*P(2,9) - P(9,19));
-Kfusion(10) = HK83*(-HK23*P(1,10) - HK25*P(10,17) + HK27*P(0,10) + HK29*P(10,18) + HK32*P(10,16) - HK34*P(3,10) + HK37*P(2,10) - P(10,19));
-Kfusion(11) = HK83*(-HK23*P(1,11) - HK25*P(11,17) + HK27*P(0,11) + HK29*P(11,18) + HK32*P(11,16) - HK34*P(3,11) + HK37*P(2,11) - P(11,19));
-Kfusion(12) = HK83*(-HK23*P(1,12) - HK25*P(12,17) + HK27*P(0,12) + HK29*P(12,18) + HK32*P(12,16) - HK34*P(3,12) + HK37*P(2,12) - P(12,19));
-Kfusion(13) = HK83*(-HK23*P(1,13) - HK25*P(13,17) + HK27*P(0,13) + HK29*P(13,18) + HK32*P(13,16) - HK34*P(3,13) + HK37*P(2,13) - P(13,19));
-Kfusion(14) = HK83*(-HK23*P(1,14) - HK25*P(14,17) + HK27*P(0,14) + HK29*P(14,18) + HK32*P(14,16) - HK34*P(3,14) + HK37*P(2,14) - P(14,19));
-Kfusion(15) = HK83*(-HK23*P(1,15) - HK25*P(15,17) + HK27*P(0,15) + HK29*P(15,18) + HK32*P(15,16) - HK34*P(3,15) + HK37*P(2,15) - P(15,19));
-Kfusion(16) = HK83*(-HK40 - HK41 + HK42 + HK43 - HK44 + HK45 + HK46 - P(16,19));
-Kfusion(17) = HK83*(-HK54 - HK55 + HK56 + HK57 - HK58 + HK59 + HK60 - P(17,19));
-Kfusion(18) = HK83*(-HK68 - HK69 + HK70 + HK71 - HK72 + HK73 + HK74 - P(18,19));
-Kfusion(19) = HK82*HK83;
-Kfusion(20) = HK83*(-HK23*P(1,20) - HK25*P(17,20) + HK27*P(0,20) + HK29*P(18,20) + HK32*P(16,20) - HK34*P(3,20) + HK37*P(2,20) - P(19,20));
-Kfusion(21) = HK83*(-HK23*P(1,21) - HK25*P(17,21) + HK27*P(0,21) + HK29*P(18,21) + HK32*P(16,21) - HK34*P(3,21) + HK37*P(2,21) + HK84);
-Kfusion(22) = HK83*(-HK23*P(1,22) - HK25*P(17,22) + HK27*P(0,22) + HK29*P(18,22) + HK32*P(16,22) - HK34*P(3,22) + HK37*P(2,22) - P(19,22));
-Kfusion(23) = HK83*(-HK23*P(1,23) - HK25*P(17,23) + HK27*P(0,23) + HK29*P(18,23) + HK32*P(16,23) - HK34*P(3,23) + HK37*P(2,23) - P(19,23));
+Kfusion(0) = HK36*HK50;
+Kfusion(1) = HK48*HK50;
+Kfusion(2) = HK41*HK50;
+Kfusion(3) = HK45*HK50;
+Kfusion(4) = HK50*(HK21*P(4,16) + HK27*P(4,17) - HK28*P(4,18) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4) + P(4,19));
+Kfusion(5) = HK50*(HK21*P(5,16) + HK27*P(5,17) - HK28*P(5,18) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5) + P(5,19));
+Kfusion(6) = HK50*(HK21*P(6,16) + HK27*P(6,17) - HK28*P(6,18) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6) + P(6,19));
+Kfusion(7) = HK50*(HK21*P(7,16) + HK27*P(7,17) - HK28*P(7,18) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7) + P(7,19));
+Kfusion(8) = HK50*(HK21*P(8,16) + HK27*P(8,17) - HK28*P(8,18) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8) + P(8,19));
+Kfusion(9) = HK50*(HK21*P(9,16) + HK27*P(9,17) - HK28*P(9,18) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9) + P(9,19));
+Kfusion(10) = HK50*(HK21*P(10,16) + HK27*P(10,17) - HK28*P(10,18) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10) + P(10,19));
+Kfusion(11) = HK50*(HK21*P(11,16) + HK27*P(11,17) - HK28*P(11,18) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11) + P(11,19));
+Kfusion(12) = HK50*(HK21*P(12,16) + HK27*P(12,17) - HK28*P(12,18) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12) + P(12,19));
+Kfusion(13) = HK50*(HK21*P(13,16) + HK27*P(13,17) - HK28*P(13,18) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13) + P(13,19));
+Kfusion(14) = HK50*(HK21*P(14,16) + HK27*P(14,17) - HK28*P(14,18) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14) + P(14,19));
+Kfusion(15) = HK50*(HK21*P(15,16) + HK27*P(15,17) - HK28*P(15,18) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15) + P(15,19));
+Kfusion(16) = HK37*HK50;
+Kfusion(17) = HK42*HK50;
+Kfusion(18) = HK38*HK50;
+Kfusion(19) = HK49*HK50;
+Kfusion(20) = HK50*(HK21*P(16,20) + HK27*P(17,20) - HK28*P(18,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20) + P(19,20));
+Kfusion(21) = HK50*(HK21*P(16,21) + HK27*P(17,21) - HK28*P(18,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21) + P(19,21));
+Kfusion(22) = HK50*(HK21*P(16,22) + HK27*P(17,22) - HK28*P(18,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22) + P(19,22));
+Kfusion(23) = HK50*(HK21*P(16,23) + HK27*P(17,23) - HK28*P(18,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23) + P(19,23));
+Kfusion(24) = HK50*(HK21*P(16,24) + HK27*P(17,24) - HK28*P(18,24) + HK29*P(1,24) + HK30*P(0,24) - HK32*P(2,24) + HK34*P(3,24) + P(19,24));
 
 
 // Observation Jacobians - axis 1
-Hfusion.at<0>() = 2*HK86;
-Hfusion.at<1>() = 2*HK87;
-Hfusion.at<2>() = 2*HK89;
-Hfusion.at<3>() = 2*HK1 - 2*HK90 - 2*HK91;
+Hfusion.at<0>() = HK14;
+Hfusion.at<1>() = HK51;
+Hfusion.at<2>() = HK6;
+Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -247,48 +159,50 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = -2*HK18 + 2*HK19;
-Hfusion.at<17>() = HK17 + HK93;
-Hfusion.at<18>() = 2*HK96;
+Hfusion.at<16>() = -2*HK22 + 2*HK23;
+Hfusion.at<17>() = HK53;
+Hfusion.at<18>() = 2*HK56;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 1;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 1
-Kfusion(0) = HK104*HK112;
-Kfusion(1) = HK110*HK112;
-Kfusion(2) = HK108*HK112;
-Kfusion(3) = HK107*HK112;
-Kfusion(4) = HK112*(-HK100*P(4,16) + HK101*P(1,4) - HK102*P(4,17) - HK103*P(3,4) + HK97*P(4,18) + HK98*P(2,4) + HK99*P(0,4) + P(4,20));
-Kfusion(5) = HK112*(-HK100*P(5,16) + HK101*P(1,5) - HK102*P(5,17) - HK103*P(3,5) + HK97*P(5,18) + HK98*P(2,5) + HK99*P(0,5) + P(5,20));
-Kfusion(6) = HK112*(-HK100*P(6,16) + HK101*P(1,6) - HK102*P(6,17) - HK103*P(3,6) + HK97*P(6,18) + HK98*P(2,6) + HK99*P(0,6) + P(6,20));
-Kfusion(7) = HK112*(-HK100*P(7,16) + HK101*P(1,7) - HK102*P(7,17) - HK103*P(3,7) + HK97*P(7,18) + HK98*P(2,7) + HK99*P(0,7) + P(7,20));
-Kfusion(8) = HK112*(-HK100*P(8,16) + HK101*P(1,8) - HK102*P(8,17) - HK103*P(3,8) + HK97*P(8,18) + HK98*P(2,8) + HK99*P(0,8) + P(8,20));
-Kfusion(9) = HK112*(-HK100*P(9,16) + HK101*P(1,9) - HK102*P(9,17) - HK103*P(3,9) + HK97*P(9,18) + HK98*P(2,9) + HK99*P(0,9) + P(9,20));
-Kfusion(10) = HK112*(-HK100*P(10,16) + HK101*P(1,10) - HK102*P(10,17) - HK103*P(3,10) + HK97*P(10,18) + HK98*P(2,10) + HK99*P(0,10) + P(10,20));
-Kfusion(11) = HK112*(-HK100*P(11,16) + HK101*P(1,11) - HK102*P(11,17) - HK103*P(3,11) + HK97*P(11,18) + HK98*P(2,11) + HK99*P(0,11) + P(11,20));
-Kfusion(12) = HK112*(-HK100*P(12,16) + HK101*P(1,12) - HK102*P(12,17) - HK103*P(3,12) + HK97*P(12,18) + HK98*P(2,12) + HK99*P(0,12) + P(12,20));
-Kfusion(13) = HK112*(-HK100*P(13,16) + HK101*P(1,13) - HK102*P(13,17) - HK103*P(3,13) + HK97*P(13,18) + HK98*P(2,13) + HK99*P(0,13) + P(13,20));
-Kfusion(14) = HK112*(-HK100*P(14,16) + HK101*P(1,14) - HK102*P(14,17) - HK103*P(3,14) + HK97*P(14,18) + HK98*P(2,14) + HK99*P(0,14) + P(14,20));
-Kfusion(15) = HK112*(-HK100*P(15,16) + HK101*P(1,15) - HK102*P(15,17) - HK103*P(3,15) + HK97*P(15,18) + HK98*P(2,15) + HK99*P(0,15) + P(15,20));
-Kfusion(16) = HK106*HK112;
-Kfusion(17) = HK105*HK112;
-Kfusion(18) = HK109*HK112;
-Kfusion(19) = HK112*(-HK100*P(16,19) + HK101*P(1,19) - HK102*P(17,19) - HK103*P(3,19) + HK97*P(18,19) + HK98*P(2,19) + HK99*P(0,19) + P(19,20));
-Kfusion(20) = HK111*HK112;
-Kfusion(21) = HK112*(-HK100*P(16,21) + HK101*P(1,21) - HK102*P(17,21) - HK103*P(3,21) + HK97*P(18,21) + HK98*P(2,21) + HK99*P(0,21) + P(20,21));
-Kfusion(22) = HK112*(-HK100*P(16,22) + HK101*P(1,22) - HK102*P(17,22) - HK103*P(3,22) + HK97*P(18,22) + HK98*P(2,22) + HK99*P(0,22) + P(20,22));
-Kfusion(23) = HK112*(-HK100*P(16,23) + HK101*P(1,23) - HK102*P(17,23) - HK103*P(3,23) + HK97*P(18,23) + HK98*P(2,23) + HK99*P(0,23) + P(20,23));
+Kfusion(0) = HK60*HK71;
+Kfusion(1) = HK67*HK71;
+Kfusion(2) = HK69*HK71;
+Kfusion(3) = HK64*HK71;
+Kfusion(4) = HK71*(HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK53*P(4,17) + HK57*P(4,18) - HK58*P(4,16) + P(4,20));
+Kfusion(5) = HK71*(HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK53*P(5,17) + HK57*P(5,18) - HK58*P(5,16) + P(5,20));
+Kfusion(6) = HK71*(HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK53*P(6,17) + HK57*P(6,18) - HK58*P(6,16) + P(6,20));
+Kfusion(7) = HK71*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK53*P(7,17) + HK57*P(7,18) - HK58*P(7,16) + P(7,20));
+Kfusion(8) = HK71*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK53*P(8,17) + HK57*P(8,18) - HK58*P(8,16) + P(8,20));
+Kfusion(9) = HK71*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK53*P(9,17) + HK57*P(9,18) - HK58*P(9,16) + P(9,20));
+Kfusion(10) = HK71*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK53*P(10,17) + HK57*P(10,18) - HK58*P(10,16) + P(10,20));
+Kfusion(11) = HK71*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK53*P(11,17) + HK57*P(11,18) - HK58*P(11,16) + P(11,20));
+Kfusion(12) = HK71*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK53*P(12,17) + HK57*P(12,18) - HK58*P(12,16) + P(12,20));
+Kfusion(13) = HK71*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK53*P(13,17) + HK57*P(13,18) - HK58*P(13,16) + P(13,20));
+Kfusion(14) = HK71*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK53*P(14,17) + HK57*P(14,18) - HK58*P(14,16) + P(14,20));
+Kfusion(15) = HK71*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK53*P(15,17) + HK57*P(15,18) - HK58*P(15,16) + P(15,20));
+Kfusion(16) = HK62*HK71;
+Kfusion(17) = HK61*HK71;
+Kfusion(18) = HK65*HK71;
+Kfusion(19) = HK71*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK53*P(17,19) + HK57*P(18,19) - HK58*P(16,19) + P(19,20));
+Kfusion(20) = HK70*HK71;
+Kfusion(21) = HK71*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK53*P(17,21) + HK57*P(18,21) - HK58*P(16,21) + P(20,21));
+Kfusion(22) = HK71*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK53*P(17,22) + HK57*P(18,22) - HK58*P(16,22) + P(20,22));
+Kfusion(23) = HK71*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK53*P(17,23) + HK57*P(18,23) - HK58*P(16,23) + P(20,23));
+Kfusion(24) = HK71*(HK29*P(2,24) - HK30*P(3,24) + HK32*P(1,24) + HK34*P(0,24) + HK53*P(17,24) + HK57*P(18,24) - HK58*P(16,24) + P(20,24));
 
 
 // Observation Jacobians - axis 2
-Hfusion.at<0>() = 2*HK36 + 2*HK8;
-Hfusion.at<1>() = -2*HK11 - 2*HK113 + 2*HK12;
-Hfusion.at<2>() = 2*HK114;
-Hfusion.at<3>() = 2*HK115;
+Hfusion.at<0>() = HK51;
+Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12;
+Hfusion.at<2>() = HK4;
+Hfusion.at<3>() = HK6;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -301,40 +215,42 @@ Hfusion.at<12>() = 0;
 Hfusion.at<13>() = 0;
 Hfusion.at<14>() = 0;
 Hfusion.at<15>() = 0;
-Hfusion.at<16>() = 2*HK116;
-Hfusion.at<17>() = -2*HK94 + 2*HK95;
-Hfusion.at<18>() = HK15 + HK93 + 1;
+Hfusion.at<16>() = 2*HK72;
+Hfusion.at<17>() = -2*HK54 + 2*HK55;
+Hfusion.at<18>() = HK73;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 1;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 2
-Kfusion(0) = HK174*(-HK118 - HK120 + HK122 + HK124 + HK126 - HK128 + HK130 - P(0,21));
-Kfusion(1) = HK174*(-HK166 - HK167 + HK168 + HK169 - HK170 + HK171 + HK172 - P(1,21));
-Kfusion(2) = HK174*(-HK152 - HK153 + HK154 + HK155 - HK156 + HK157 + HK158 - P(2,21));
-Kfusion(3) = HK174*(-HK138 - HK139 + HK140 + HK141 - HK142 + HK143 + HK144 - P(3,21));
-Kfusion(4) = HK174*(-HK117*P(4,16) - HK119*P(3,4) + HK121*P(0,4) + HK123*P(4,17) + HK125*P(4,18) - HK127*P(2,4) + HK129*P(1,4) - P(4,21));
-Kfusion(5) = HK174*(-HK117*P(5,16) - HK119*P(3,5) + HK121*P(0,5) + HK123*P(5,17) + HK125*P(5,18) - HK127*P(2,5) + HK129*P(1,5) - P(5,21));
-Kfusion(6) = HK174*(-HK117*P(6,16) - HK119*P(3,6) + HK121*P(0,6) + HK123*P(6,17) + HK125*P(6,18) - HK127*P(2,6) + HK129*P(1,6) - P(6,21));
-Kfusion(7) = HK174*(-HK117*P(7,16) - HK119*P(3,7) + HK121*P(0,7) + HK123*P(7,17) + HK125*P(7,18) - HK127*P(2,7) + HK129*P(1,7) - P(7,21));
-Kfusion(8) = HK174*(-HK117*P(8,16) - HK119*P(3,8) + HK121*P(0,8) + HK123*P(8,17) + HK125*P(8,18) - HK127*P(2,8) + HK129*P(1,8) - P(8,21));
-Kfusion(9) = HK174*(-HK117*P(9,16) - HK119*P(3,9) + HK121*P(0,9) + HK123*P(9,17) + HK125*P(9,18) - HK127*P(2,9) + HK129*P(1,9) - P(9,21));
-Kfusion(10) = HK174*(-HK117*P(10,16) - HK119*P(3,10) + HK121*P(0,10) + HK123*P(10,17) + HK125*P(10,18) - HK127*P(2,10) + HK129*P(1,10) - P(10,21));
-Kfusion(11) = HK174*(-HK117*P(11,16) - HK119*P(3,11) + HK121*P(0,11) + HK123*P(11,17) + HK125*P(11,18) - HK127*P(2,11) + HK129*P(1,11) - P(11,21));
-Kfusion(12) = HK174*(-HK117*P(12,16) - HK119*P(3,12) + HK121*P(0,12) + HK123*P(12,17) + HK125*P(12,18) - HK127*P(2,12) + HK129*P(1,12) - P(12,21));
-Kfusion(13) = HK174*(-HK117*P(13,16) - HK119*P(3,13) + HK121*P(0,13) + HK123*P(13,17) + HK125*P(13,18) - HK127*P(2,13) + HK129*P(1,13) - P(13,21));
-Kfusion(14) = HK174*(-HK117*P(14,16) - HK119*P(3,14) + HK121*P(0,14) + HK123*P(14,17) + HK125*P(14,18) - HK127*P(2,14) + HK129*P(1,14) - P(14,21));
-Kfusion(15) = HK174*(-HK117*P(15,16) - HK119*P(3,15) + HK121*P(0,15) + HK123*P(15,17) + HK125*P(15,18) - HK127*P(2,15) + HK129*P(1,15) - P(15,21));
-Kfusion(16) = HK174*(-HK145 - HK146 + HK147 + HK148 - HK149 + HK150 + HK151 - P(16,21));
-Kfusion(17) = HK174*(-HK159 - HK160 + HK161 + HK162 - HK163 + HK164 + HK165 - P(17,21));
-Kfusion(18) = HK174*(-HK131 - HK132 + HK133 + HK134 - HK135 + HK136 + HK137 - P(18,21));
-Kfusion(19) = HK174*(-HK117*P(16,19) - HK119*P(3,19) + HK121*P(0,19) + HK123*P(17,19) + HK125*P(18,19) - HK127*P(2,19) + HK129*P(1,19) + HK84);
-Kfusion(20) = HK174*(-HK117*P(16,20) - HK119*P(3,20) + HK121*P(0,20) + HK123*P(17,20) + HK125*P(18,20) - HK127*P(2,20) + HK129*P(1,20) - P(20,21));
-Kfusion(21) = HK173*HK174;
-Kfusion(22) = HK174*(-HK117*P(16,22) - HK119*P(3,22) + HK121*P(0,22) + HK123*P(17,22) + HK125*P(18,22) - HK127*P(2,22) + HK129*P(1,22) - P(21,22));
-Kfusion(23) = HK174*(-HK117*P(16,23) - HK119*P(3,23) + HK121*P(0,23) + HK123*P(17,23) + HK125*P(18,23) - HK127*P(2,23) + HK129*P(1,23) - P(21,23));
+Kfusion(0) = HK76*HK84;
+Kfusion(1) = HK79*HK84;
+Kfusion(2) = HK82*HK84;
+Kfusion(3) = HK81*HK84;
+Kfusion(4) = HK84*(HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK73*P(4,18) + HK74*P(4,16) - HK75*P(4,17) + P(4,21));
+Kfusion(5) = HK84*(HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK73*P(5,18) + HK74*P(5,16) - HK75*P(5,17) + P(5,21));
+Kfusion(6) = HK84*(HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK73*P(6,18) + HK74*P(6,16) - HK75*P(6,17) + P(6,21));
+Kfusion(7) = HK84*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK73*P(7,18) + HK74*P(7,16) - HK75*P(7,17) + P(7,21));
+Kfusion(8) = HK84*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK73*P(8,18) + HK74*P(8,16) - HK75*P(8,17) + P(8,21));
+Kfusion(9) = HK84*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK73*P(9,18) + HK74*P(9,16) - HK75*P(9,17) + P(9,21));
+Kfusion(10) = HK84*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK73*P(10,18) + HK74*P(10,16) - HK75*P(10,17) + P(10,21));
+Kfusion(11) = HK84*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK73*P(11,18) + HK74*P(11,16) - HK75*P(11,17) + P(11,21));
+Kfusion(12) = HK84*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK73*P(12,18) + HK74*P(12,16) - HK75*P(12,17) + P(12,21));
+Kfusion(13) = HK84*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK73*P(13,18) + HK74*P(13,16) - HK75*P(13,17) + P(13,21));
+Kfusion(14) = HK84*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK73*P(14,18) + HK74*P(14,16) - HK75*P(14,17) + P(14,21));
+Kfusion(15) = HK84*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK73*P(15,18) + HK74*P(15,16) - HK75*P(15,17) + P(15,21));
+Kfusion(16) = HK80*HK84;
+Kfusion(17) = HK78*HK84;
+Kfusion(18) = HK77*HK84;
+Kfusion(19) = HK84*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK73*P(18,19) + HK74*P(16,19) - HK75*P(17,19) + P(19,21));
+Kfusion(20) = HK84*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK73*P(18,20) + HK74*P(16,20) - HK75*P(17,20) + P(20,21));
+Kfusion(21) = HK83*HK84;
+Kfusion(22) = HK84*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK73*P(18,22) + HK74*P(16,22) - HK75*P(17,22) + P(21,22));
+Kfusion(23) = HK84*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK73*P(18,23) + HK74*P(16,23) - HK75*P(17,23) + P(21,23));
+Kfusion(24) = HK84*(HK29*P(3,24) + HK30*P(2,24) + HK32*P(0,24) - HK34*P(1,24) + HK73*P(18,24) + HK74*P(16,24) - HK75*P(17,24) + P(21,24));
 
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp

@@ -2,34 +2,28 @@
 const float IV0 = q0*q1;
 const float IV1 = q2*q3;
 const float IV2 = 2*IV0 + 2*IV1;
-const float IV3 = magN*q1;
-const float IV4 = 2*IV3 + 2*magD*q3;
-const float IV5 = magD*q1;
-const float IV6 = -magN*q3;
-const float IV7 = 2*IV5 + 2*IV6;
-const float IV8 = 2*q0*q3 - 2*q1*q2;
-const float IV9 = magN*q2;
-const float IV10 = magE*q1;
-const float IV11 = -4*IV10 + 2*IV9 + 2*magD*q0;
-const float IV12 = 2*powf(q1, 2) - 1;
-const float IV13 = IV12 + 2*powf(q3, 2);
-const float IV14 = magN*q0;
-const float IV15 = magD*q2;
-const float IV16 = magE*q3;
-const float IV17 = 2*IV14 - 2*IV15 + 4*IV16;
-const float IV18 = 2*q0*q2 + 2*q1*q3;
-const float IV19 = 2*IV3 + 2*magE*q2;
-const float IV20 = 2*IV10 - 2*IV9;
-const float IV21 = 2*IV0 - 2*IV1;
-const float IV22 = 2*IV14 - 4*IV15 + 2*IV16;
-const float IV23 = 4*IV5 + 2*IV6 + 2*magE*q0;
-const float IV24 = IV12 + 2*powf(q2, 2);
+const float IV3 = 2*q0*q3 - 2*q1*q2;
+const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
+const float IV8 = powf(q2, 2);
+const float IV9 = powf(q3, 2);
+const float IV10 = powf(q0, 2) - powf(q1, 2);
+const float IV11 = IV10 + IV8 - IV9;
+const float IV12 = IV7*P(2,3);
+const float IV13 = IV5*P(0,1);
+const float IV14 = IV6*P(0,1);
+const float IV15 = IV4*P(2,3);
+const float IV16 = 2*q0*q2 + 2*q1*q3;
+const float IV17 = 2*IV0 - 2*IV1;
+const float IV18 = IV10 - IV8 + IV9;
 
 
 // Observation Jacobians - axis 0
 Hfusion.at<0>() = R_MAG;
-Hfusion.at<1>() = IV11*P(1,20) + IV11*(IV11*P(1,1) - IV13*P(1,17) - IV17*P(1,3) + IV2*P(1,18) + IV4*P(1,2) + IV7*P(0,1) - IV8*P(1,16) + P(1,20)) - IV13*P(17,20) - IV13*(IV11*P(1,17) - IV13*P(17,17) - IV17*P(3,17) + IV2*P(17,18) + IV4*P(2,17) + IV7*P(0,17) - IV8*P(16,17) + P(17,20)) - IV17*P(3,20) - IV17*(IV11*P(1,3) - IV13*P(3,17) - IV17*P(3,3) + IV2*P(3,18) + IV4*P(2,3) + IV7*P(0,3) - IV8*P(3,16) + P(3,20)) + IV2*P(18,20) + IV2*(IV11*P(1,18) - IV13*P(17,18) - IV17*P(3,18) + IV2*P(18,18) + IV4*P(2,18) + IV7*P(0,18) - IV8*P(16,18) + P(18,20)) + IV4*P(2,20) + IV4*(IV11*P(1,2) - IV13*P(2,17) - IV17*P(2,3) + IV2*P(2,18) + IV4*P(2,2) + IV7*P(0,2) - IV8*P(2,16) + P(2,20)) + IV7*P(0,20) + IV7*(IV11*P(0,1) - IV13*P(0,17) - IV17*P(0,3) + IV2*P(0,18) + IV4*P(0,2) + IV7*P(0,0) - IV8*P(0,16) + P(0,20)) - IV8*P(16,20) - IV8*(IV11*P(1,16) - IV13*P(16,17) - IV17*P(3,16) + IV2*P(16,18) + IV4*P(2,16) + IV7*P(0,16) - IV8*P(16,16) + P(16,20)) + P(20,20) + R_MAG;
-Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,16) - IV21*P(16,17) + IV22*P(2,16) - IV23*P(1,16) - IV24*P(16,18) + P(16,21)) + IV19*P(3,21) + IV19*(IV18*P(3,16) + IV19*P(3,3) - IV20*P(0,3) - IV21*P(3,17) + IV22*P(2,3) - IV23*P(1,3) - IV24*P(3,18) + P(3,21)) - IV20*P(0,21) - IV20*(IV18*P(0,16) + IV19*P(0,3) - IV20*P(0,0) - IV21*P(0,17) + IV22*P(0,2) - IV23*P(0,1) - IV24*P(0,18) + P(0,21)) - IV21*P(17,21) - IV21*(IV18*P(16,17) + IV19*P(3,17) - IV20*P(0,17) - IV21*P(17,17) + IV22*P(2,17) - IV23*P(1,17) - IV24*P(17,18) + P(17,21)) + IV22*P(2,21) + IV22*(IV18*P(2,16) + IV19*P(2,3) - IV20*P(0,2) - IV21*P(2,17) + IV22*P(2,2) - IV23*P(1,2) - IV24*P(2,18) + P(2,21)) - IV23*P(1,21) - IV23*(IV18*P(1,16) + IV19*P(1,3) - IV20*P(0,1) - IV21*P(1,17) + IV22*P(1,2) - IV23*P(1,1) - IV24*P(1,18) + P(1,21)) - IV24*P(18,21) - IV24*(IV18*P(16,18) + IV19*P(3,18) - IV20*P(0,18) - IV21*P(17,18) + IV22*P(2,18) - IV23*P(1,18) - IV24*P(18,18) + P(18,21)) + P(21,21) + R_MAG;
+Hfusion.at<1>() = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
+Hfusion.at<2>() = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
 
 
 // Kalman gains - axis 0

src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp

@@ -4,25 +4,25 @@
 #include "../../../../../matrix/matrix/math.hpp"
 #include "util.h"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
 
 int main()
 {
     // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
 
-	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
-    Vector24f H_ACC;
-    Vector24f Kfusion;
+	SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
+    Vector25f H_ACC;
+    Vector25f Kfusion;
     float drag_innov_var;
 
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
+    Vector25f Hfusion_sympy;
+    Vector25f Kfusion_sympy;
 
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
+    Vector25f Hfusion_matlab;
+    Vector25f Kfusion_matlab;
 
 	float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
 	float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
@@ -56,7 +56,7 @@ int main()
     const float rho = 1.225f;
 
     // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {

src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp

@@ -1,61 +1,48 @@
 // Axis 0 equations
 // Sub Expressions
-const float HK0 = q2*vd;
+const float HK0 = vn - vwn;
 const float HK1 = ve - vwe;
-const float HK2 = HK1*q3;
-const float HK3 = HK0 - HK2;
-const float HK4 = 2*Kaccx;
-const float HK5 = HK1*q2 + q3*vd;
-const float HK6 = q0*vd;
-const float HK7 = HK1*q1;
-const float HK8 = 2*vn - 2*vwn;
-const float HK9 = HK8*q2;
-const float HK10 = HK6 - HK7 + HK9;
-const float HK11 = HK1*q0 - HK8*q3 + q1*vd;
-const float HK12 = 2*powf(q2, 2);
-const float HK13 = 2*powf(q3, 2);
-const float HK14 = HK12 + HK13 - 1;
-const float HK15 = HK14*Kaccx;
-const float HK16 = q0*q3 + q1*q2;
-const float HK17 = HK16*HK4;
-const float HK18 = q0*q2;
-const float HK19 = q1*q3;
-const float HK20 = HK18 - HK19;
-const float HK21 = 2*HK3;
-const float HK22 = 2*HK10;
-const float HK23 = 2*HK20;
-const float HK24 = 2*HK16;
-const float HK25 = 2*HK5;
-const float HK26 = 2*HK11;
-const float HK27 = HK14*P(0,22) - HK24*P(0,23) + HK24*P(0,5) + HK25*P(0,1) + HK26*P(0,3);
-const float HK28 = -HK12 - HK13 + 1;
-const float HK29 = -2*HK0 + 2*HK2;
-const float HK30 = -2*HK6 + 2*HK7 - 2*HK9;
-const float HK31 = -2*HK18 + 2*HK19;
-const float HK32 = HK24*P(5,23);
-const float HK33 = HK14*P(22,23) - HK24*P(23,23) + HK25*P(1,23) + HK26*P(3,23) + HK32;
-const float HK34 = powf(Kaccx, 2);
-const float HK35 = HK24*HK34;
-const float HK36 = HK14*P(5,22) + HK24*P(5,5) + HK25*P(1,5) + HK26*P(3,5) - HK32;
-const float HK37 = HK14*P(6,22) + HK24*P(5,6) - HK24*P(6,23) + HK25*P(1,6) + HK26*P(3,6);
-const float HK38 = HK14*P(1,22) - HK24*P(1,23) + HK24*P(1,5) + HK25*P(1,1) + HK26*P(1,3);
-const float HK39 = HK14*P(4,22);
-const float HK40 = -HK24*P(4,23) + HK24*P(4,5) + HK25*P(1,4) + HK26*P(3,4) + HK39;
-const float HK41 = HK14*HK34;
-const float HK42 = HK14*P(22,22) - HK24*P(22,23) + HK24*P(5,22) + HK25*P(1,22) + HK26*P(3,22);
-const float HK43 = HK14*P(3,22) - HK24*P(3,23) + HK24*P(3,5) + HK25*P(1,3) + HK26*P(3,3);
-const float HK44 = HK14*P(2,22) - HK24*P(2,23) + HK24*P(2,5) + HK25*P(1,2) + HK26*P(2,3);
-const float HK45 = Kaccx/(HK21*HK34*(HK27 + HK28*P(0,4) + HK29*P(0,0) + HK30*P(0,2) + HK31*P(0,6)) + HK22*HK34*(HK28*P(2,4) + HK29*P(0,2) + HK30*P(2,2) + HK31*P(2,6) + HK44) + HK23*HK34*(HK28*P(4,6) + HK29*P(0,6) + HK30*P(2,6) + HK31*P(6,6) + HK37) - HK25*HK34*(HK28*P(1,4) + HK29*P(0,1) + HK30*P(1,2) + HK31*P(1,6) + HK38) - HK26*HK34*(HK28*P(3,4) + HK29*P(0,3) + HK30*P(2,3) + HK31*P(3,6) + HK43) + HK35*(HK28*P(4,23) + HK29*P(0,23) + HK30*P(2,23) + HK31*P(6,23) + HK33) - HK35*(HK28*P(4,5) + HK29*P(0,5) + HK30*P(2,5) + HK31*P(5,6) + HK36) - HK41*(HK28*P(4,22) + HK29*P(0,22) + HK30*P(2,22) + HK31*P(6,22) + HK42) + HK41*(HK28*P(4,4) + HK29*P(0,4) + HK30*P(2,4) + HK31*P(4,6) + HK40) - R_ACC);
+const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+const float HK3 = 2*Kaccx;
+const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
+const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
+const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
+const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
+const float HK8 = HK7*Kaccx;
+const float HK9 = q0*q3 + q1*q2;
+const float HK10 = HK3*HK9;
+const float HK11 = q0*q2 - q1*q3;
+const float HK12 = 2*HK9;
+const float HK13 = 2*HK11;
+const float HK14 = 2*HK4;
+const float HK15 = 2*HK2;
+const float HK16 = 2*HK5;
+const float HK17 = 2*HK6;
+const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4);
+const float HK19 = HK12*P(5,23);
+const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23);
+const float HK21 = powf(Kaccx, 2);
+const float HK22 = HK12*HK21;
+const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22);
+const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22);
+const float HK25 = HK7*P(4,22);
+const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4);
+const float HK27 = HK21*HK7;
+const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22);
+const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4);
+const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4);
+const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4);
+const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = HK3*HK4;
-Hfusion.at<1>() = -HK4*HK5;
-Hfusion.at<2>() = HK10*HK4;
-Hfusion.at<3>() = -HK11*HK4;
-Hfusion.at<4>() = HK15;
-Hfusion.at<5>() = -HK17;
-Hfusion.at<6>() = HK20*HK4;
+Hfusion.at<0>() = -HK2*HK3;
+Hfusion.at<1>() = -HK3*HK4;
+Hfusion.at<2>() = HK3*HK5;
+Hfusion.at<3>() = -HK3*HK6;
+Hfusion.at<4>() = -HK8;
+Hfusion.at<5>() = -HK10;
+Hfusion.at<6>() = HK11*HK3;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -71,82 +58,84 @@ Hfusion.at<18>() = 0;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
-Hfusion.at<22>() = -HK15;
-Hfusion.at<23>() = HK17;
+Hfusion.at<22>() = HK8;
+Hfusion.at<23>() = HK10;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HK45*(-HK14*P(0,4) - HK21*P(0,0) - HK22*P(0,2) - HK23*P(0,6) + HK27);
-Kfusion(1) = HK45*(-HK14*P(1,4) - HK21*P(0,1) - HK22*P(1,2) - HK23*P(1,6) + HK38);
-Kfusion(2) = HK45*(-HK14*P(2,4) - HK21*P(0,2) - HK22*P(2,2) - HK23*P(2,6) + HK44);
-Kfusion(3) = HK45*(-HK14*P(3,4) - HK21*P(0,3) - HK22*P(2,3) - HK23*P(3,6) + HK43);
-Kfusion(4) = HK45*(-HK14*P(4,4) - HK21*P(0,4) - HK22*P(2,4) - HK23*P(4,6) + HK40);
-Kfusion(5) = HK45*(-HK14*P(4,5) - HK21*P(0,5) - HK22*P(2,5) - HK23*P(5,6) + HK36);
-Kfusion(6) = HK45*(-HK14*P(4,6) - HK21*P(0,6) - HK22*P(2,6) - HK23*P(6,6) + HK37);
-Kfusion(7) = HK45*(-HK14*P(4,7) + HK14*P(7,22) - HK21*P(0,7) - HK22*P(2,7) - HK23*P(6,7) + HK24*P(5,7) - HK24*P(7,23) + HK25*P(1,7) + HK26*P(3,7));
-Kfusion(8) = HK45*(-HK14*P(4,8) + HK14*P(8,22) - HK21*P(0,8) - HK22*P(2,8) - HK23*P(6,8) + HK24*P(5,8) - HK24*P(8,23) + HK25*P(1,8) + HK26*P(3,8));
-Kfusion(9) = HK45*(-HK14*P(4,9) + HK14*P(9,22) - HK21*P(0,9) - HK22*P(2,9) - HK23*P(6,9) + HK24*P(5,9) - HK24*P(9,23) + HK25*P(1,9) + HK26*P(3,9));
-Kfusion(10) = HK45*(HK14*P(10,22) - HK14*P(4,10) - HK21*P(0,10) - HK22*P(2,10) - HK23*P(6,10) - HK24*P(10,23) + HK24*P(5,10) + HK25*P(1,10) + HK26*P(3,10));
-Kfusion(11) = HK45*(HK14*P(11,22) - HK14*P(4,11) - HK21*P(0,11) - HK22*P(2,11) - HK23*P(6,11) - HK24*P(11,23) + HK24*P(5,11) + HK25*P(1,11) + HK26*P(3,11));
-Kfusion(12) = HK45*(HK14*P(12,22) - HK14*P(4,12) - HK21*P(0,12) - HK22*P(2,12) - HK23*P(6,12) - HK24*P(12,23) + HK24*P(5,12) + HK25*P(1,12) + HK26*P(3,12));
-Kfusion(13) = HK45*(HK14*P(13,22) - HK14*P(4,13) - HK21*P(0,13) - HK22*P(2,13) - HK23*P(6,13) - HK24*P(13,23) + HK24*P(5,13) + HK25*P(1,13) + HK26*P(3,13));
-Kfusion(14) = HK45*(HK14*P(14,22) - HK14*P(4,14) - HK21*P(0,14) - HK22*P(2,14) - HK23*P(6,14) - HK24*P(14,23) + HK24*P(5,14) + HK25*P(1,14) + HK26*P(3,14));
-Kfusion(15) = HK45*(HK14*P(15,22) - HK14*P(4,15) - HK21*P(0,15) - HK22*P(2,15) - HK23*P(6,15) - HK24*P(15,23) + HK24*P(5,15) + HK25*P(1,15) + HK26*P(3,15));
-Kfusion(16) = HK45*(HK14*P(16,22) - HK14*P(4,16) - HK21*P(0,16) - HK22*P(2,16) - HK23*P(6,16) - HK24*P(16,23) + HK24*P(5,16) + HK25*P(1,16) + HK26*P(3,16));
-Kfusion(17) = HK45*(HK14*P(17,22) - HK14*P(4,17) - HK21*P(0,17) - HK22*P(2,17) - HK23*P(6,17) - HK24*P(17,23) + HK24*P(5,17) + HK25*P(1,17) + HK26*P(3,17));
-Kfusion(18) = HK45*(HK14*P(18,22) - HK14*P(4,18) - HK21*P(0,18) - HK22*P(2,18) - HK23*P(6,18) - HK24*P(18,23) + HK24*P(5,18) + HK25*P(1,18) + HK26*P(3,18));
-Kfusion(19) = HK45*(HK14*P(19,22) - HK14*P(4,19) - HK21*P(0,19) - HK22*P(2,19) - HK23*P(6,19) - HK24*P(19,23) + HK24*P(5,19) + HK25*P(1,19) + HK26*P(3,19));
-Kfusion(20) = HK45*(HK14*P(20,22) - HK14*P(4,20) - HK21*P(0,20) - HK22*P(2,20) - HK23*P(6,20) - HK24*P(20,23) + HK24*P(5,20) + HK25*P(1,20) + HK26*P(3,20));
-Kfusion(21) = HK45*(HK14*P(21,22) - HK14*P(4,21) - HK21*P(0,21) - HK22*P(2,21) - HK23*P(6,21) - HK24*P(21,23) + HK24*P(5,21) + HK25*P(1,21) + HK26*P(3,21));
-Kfusion(22) = HK45*(-HK21*P(0,22) - HK22*P(2,22) - HK23*P(6,22) - HK39 + HK42);
-Kfusion(23) = HK45*(-HK14*P(4,23) - HK21*P(0,23) - HK22*P(2,23) - HK23*P(6,23) + HK33);
+Kfusion(0) = -HK18*HK32;
+Kfusion(1) = -HK29*HK32;
+Kfusion(2) = -HK30*HK32;
+Kfusion(3) = -HK31*HK32;
+Kfusion(4) = -HK26*HK32;
+Kfusion(5) = -HK23*HK32;
+Kfusion(6) = -HK24*HK32;
+Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
+Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
+Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
+Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
+Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
+Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
+Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
+Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
+Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
+Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
+Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
+Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
+Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
+Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
+Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
+Kfusion(22) = -HK28*HK32;
+Kfusion(23) = -HK20*HK32;
+Kfusion(24) = -HK32*(-HK12*P(23,24) + HK12*P(5,24) - HK13*P(6,24) + HK14*P(1,24) + HK15*P(0,24) - HK16*P(2,24) + HK17*P(3,24) - HK7*P(22,24) + HK7*P(4,24));
 
 
 // Axis 1 equations
 // Sub Expressions
-const float HK0 = vn - vwn;
-const float HK1 = -HK0*q3 + q1*vd;
-const float HK2 = 2*Kaccy;
-const float HK3 = 2*ve - 2*vwe;
-const float HK4 = HK0*q2 - HK3*q1 + q0*vd;
-const float HK5 = HK0*q1 + q3*vd;
-const float HK6 = HK0*q0 + HK3*q3 - q2*vd;
+const float HK0 = ve - vwe;
+const float HK1 = vn - vwn;
+const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
+const float HK3 = 2*Kaccy;
+const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
+const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
+const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
 const float HK7 = q0*q3 - q1*q2;
-const float HK8 = HK2*HK7;
-const float HK9 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
+const float HK8 = HK3*HK7;
+const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
 const float HK10 = HK9*Kaccy;
 const float HK11 = q0*q1 + q2*q3;
 const float HK12 = 2*HK11;
 const float HK13 = 2*HK7;
 const float HK14 = 2*HK5;
-const float HK15 = 2*HK1;
+const float HK15 = 2*HK2;
 const float HK16 = 2*HK4;
 const float HK17 = 2*HK6;
-const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) + HK9*P(0,23) - HK9*P(0,5);
+const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5);
 const float HK19 = powf(Kaccy, 2);
-const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) - HK9*P(5,6) + HK9*P(6,23);
+const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23);
 const float HK21 = HK13*P(4,22);
-const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 + HK9*P(22,23) - HK9*P(5,22);
+const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22);
 const float HK23 = HK13*HK19;
-const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 + HK9*P(4,23) - HK9*P(4,5);
-const float HK25 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) + HK9*P(2,23) - HK9*P(2,5);
-const float HK26 = HK9*P(5,23);
-const float HK27 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) - HK26 + HK9*P(23,23);
-const float HK28 = HK19*HK9;
-const float HK29 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) + HK26 - HK9*P(5,5);
-const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) + HK9*P(1,23) - HK9*P(1,5);
-const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) + HK9*P(3,23) - HK9*P(3,5);
-const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK25 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK27*HK28 - HK28*HK29 + R_ACC);
+const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5);
+const float HK25 = HK9*P(5,23);
+const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5);
+const float HK27 = HK19*HK9;
+const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23);
+const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5);
+const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5);
+const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5);
+const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = -HK1*HK2;
-Hfusion.at<1>() = -HK2*HK4;
-Hfusion.at<2>() = -HK2*HK5;
-Hfusion.at<3>() = HK2*HK6;
+Hfusion.at<0>() = -HK2*HK3;
+Hfusion.at<1>() = -HK3*HK4;
+Hfusion.at<2>() = -HK3*HK5;
+Hfusion.at<3>() = HK3*HK6;
 Hfusion.at<4>() = HK8;
-Hfusion.at<5>() = HK10;
-Hfusion.at<6>() = -HK11*HK2;
+Hfusion.at<5>() = -HK10;
+Hfusion.at<6>() = -HK11*HK3;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -163,31 +152,33 @@ Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = -HK8;
-Hfusion.at<23>() = -HK10;
+Hfusion.at<23>() = HK10;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
 Kfusion(0) = -HK18*HK32;
 Kfusion(1) = -HK30*HK32;
-Kfusion(2) = -HK25*HK32;
+Kfusion(2) = -HK29*HK32;
 Kfusion(3) = -HK31*HK32;
 Kfusion(4) = -HK24*HK32;
-Kfusion(5) = -HK29*HK32;
+Kfusion(5) = -HK26*HK32;
 Kfusion(6) = -HK20*HK32;
-Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) - HK9*P(5,7) + HK9*P(7,23));
-Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) - HK9*P(5,8) + HK9*P(8,23));
-Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) - HK9*P(5,9) + HK9*P(9,23));
-Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) + HK9*P(10,23) - HK9*P(5,10));
-Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) + HK9*P(11,23) - HK9*P(5,11));
-Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) + HK9*P(12,23) - HK9*P(5,12));
-Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) + HK9*P(13,23) - HK9*P(5,13));
-Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) + HK9*P(14,23) - HK9*P(5,14));
-Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) + HK9*P(15,23) - HK9*P(5,15));
-Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) + HK9*P(16,23) - HK9*P(5,16));
-Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) + HK9*P(17,23) - HK9*P(5,17));
-Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) + HK9*P(18,23) - HK9*P(5,18));
-Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) + HK9*P(19,23) - HK9*P(5,19));
-Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) + HK9*P(20,23) - HK9*P(5,20));
-Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21));
+Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23));
+Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23));
+Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23));
+Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10));
+Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11));
+Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12));
+Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13));
+Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14));
+Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15));
+Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16));
+Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17));
+Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18));
+Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19));
+Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20));
+Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21));
 Kfusion(22) = -HK22*HK32;
-Kfusion(23) = -HK27*HK32;
+Kfusion(23) = -HK28*HK32;
+Kfusion(24) = -HK32*(HK12*P(6,24) + HK13*P(22,24) - HK13*P(4,24) + HK14*P(2,24) + HK15*P(0,24) + HK16*P(1,24) - HK17*P(3,24) - HK9*P(23,24) + HK9*P(5,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp

@@ -1,99 +1,79 @@
 // Sub Expressions
-const float HK0 = q2*vd;
+const float HK0 = vn - vwn;
 const float HK1 = ve - vwe;
-const float HK2 = HK1*q3;
-const float HK3 = HK0 - HK2;
-const float HK4 = 2*Kaccx;
-const float HK5 = q3*vd;
-const float HK6 = HK1*q2 + HK5;
-const float HK7 = q0*vd;
-const float HK8 = HK1*q1;
-const float HK9 = vn - vwn;
-const float HK10 = HK9*q2;
-const float HK11 = 2*HK10;
-const float HK12 = HK11 + HK7 - HK8;
-const float HK13 = q1*vd;
-const float HK14 = HK9*q3;
-const float HK15 = HK1*q0 + HK13 - 2*HK14;
-const float HK16 = 2*powf(q2, 2);
-const float HK17 = 2*powf(q3, 2);
-const float HK18 = HK17 - 1;
-const float HK19 = HK16 + HK18;
-const float HK20 = HK19*Kaccx;
-const float HK21 = q0*q3;
-const float HK22 = q1*q2;
-const float HK23 = HK21 + HK22;
-const float HK24 = HK23*HK4;
-const float HK25 = q0*q2;
-const float HK26 = q1*q3;
-const float HK27 = HK25 - HK26;
-const float HK28 = 2*HK3;
-const float HK29 = 2*HK12;
-const float HK30 = 2*HK27;
-const float HK31 = 2*HK23;
-const float HK32 = 2*HK6;
-const float HK33 = 2*HK15;
-const float HK34 = HK19*P(0,22) - HK31*P(0,23) + HK31*P(0,5) + HK32*P(0,1) + HK33*P(0,3);
-const float HK35 = -HK16 - HK17 + 1;
-const float HK36 = -HK0;
-const float HK37 = 2*HK2 + 2*HK36;
-const float HK38 = -2*HK11 - 2*HK7 + 2*HK8;
-const float HK39 = -2*HK25 + 2*HK26;
-const float HK40 = HK31*P(5,23);
-const float HK41 = HK19*P(22,23) - HK31*P(23,23) + HK32*P(1,23) + HK33*P(3,23) + HK40;
-const float HK42 = powf(Kaccx, 2);
-const float HK43 = HK31*HK42;
-const float HK44 = HK19*P(5,22) + HK31*P(5,5) + HK32*P(1,5) + HK33*P(3,5) - HK40;
-const float HK45 = HK19*P(6,22) + HK31*P(5,6) - HK31*P(6,23) + HK32*P(1,6) + HK33*P(3,6);
-const float HK46 = HK19*P(1,22) - HK31*P(1,23) + HK31*P(1,5) + HK32*P(1,1) + HK33*P(1,3);
-const float HK47 = HK19*P(4,22);
-const float HK48 = -HK31*P(4,23) + HK31*P(4,5) + HK32*P(1,4) + HK33*P(3,4) + HK47;
-const float HK49 = HK19*HK42;
-const float HK50 = HK19*P(22,22) - HK31*P(22,23) + HK31*P(5,22) + HK32*P(1,22) + HK33*P(3,22);
-const float HK51 = HK19*P(3,22) - HK31*P(3,23) + HK31*P(3,5) + HK32*P(1,3) + HK33*P(3,3);
-const float HK52 = HK19*P(2,22) - HK31*P(2,23) + HK31*P(2,5) + HK32*P(1,2) + HK33*P(2,3);
-const float HK53 = Kaccx/(HK28*HK42*(HK34 + HK35*P(0,4) + HK37*P(0,0) + HK38*P(0,2) + HK39*P(0,6)) + HK29*HK42*(HK35*P(2,4) + HK37*P(0,2) + HK38*P(2,2) + HK39*P(2,6) + HK52) + HK30*HK42*(HK35*P(4,6) + HK37*P(0,6) + HK38*P(2,6) + HK39*P(6,6) + HK45) - HK32*HK42*(HK35*P(1,4) + HK37*P(0,1) + HK38*P(1,2) + HK39*P(1,6) + HK46) - HK33*HK42*(HK35*P(3,4) + HK37*P(0,3) + HK38*P(2,3) + HK39*P(3,6) + HK51) + HK43*(HK35*P(4,23) + HK37*P(0,23) + HK38*P(2,23) + HK39*P(6,23) + HK41) - HK43*(HK35*P(4,5) + HK37*P(0,5) + HK38*P(2,5) + HK39*P(5,6) + HK44) - HK49*(HK35*P(4,22) + HK37*P(0,22) + HK38*P(2,22) + HK39*P(6,22) + HK50) + HK49*(HK35*P(4,4) + HK37*P(0,4) + HK38*P(2,4) + HK39*P(4,6) + HK48) - R_ACC);
-const float HK54 = HK13 - HK14;
-const float HK55 = 2*Kaccy;
-const float HK56 = HK10 + HK7 - 2*HK8;
-const float HK57 = HK5 + HK9*q1;
-const float HK58 = 2*HK2 + HK36 + HK9*q0;
-const float HK59 = HK21 - HK22;
-const float HK60 = HK55*HK59;
-const float HK61 = HK18 + 2*powf(q1, 2);
-const float HK62 = HK61*Kaccy;
-const float HK63 = q0*q1 + q2*q3;
-const float HK64 = 2*HK63;
-const float HK65 = 2*HK59;
-const float HK66 = 2*HK57;
-const float HK67 = 2*HK54;
-const float HK68 = 2*HK56;
-const float HK69 = 2*HK58;
-const float HK70 = HK61*P(0,23) - HK61*P(0,5) + HK64*P(0,6) + HK65*P(0,22) - HK65*P(0,4) + HK66*P(0,2) + HK67*P(0,0) + HK68*P(0,1) - HK69*P(0,3);
-const float HK71 = powf(Kaccy, 2);
-const float HK72 = -HK61*P(5,6) + HK61*P(6,23) + HK64*P(6,6) - HK65*P(4,6) + HK65*P(6,22) + HK66*P(2,6) + HK67*P(0,6) + HK68*P(1,6) - HK69*P(3,6);
-const float HK73 = HK65*P(4,22);
-const float HK74 = HK61*P(22,23) - HK61*P(5,22) + HK64*P(6,22) + HK65*P(22,22) + HK66*P(2,22) + HK67*P(0,22) + HK68*P(1,22) - HK69*P(3,22) - HK73;
-const float HK75 = HK65*HK71;
-const float HK76 = HK61*P(4,23) - HK61*P(4,5) + HK64*P(4,6) - HK65*P(4,4) + HK66*P(2,4) + HK67*P(0,4) + HK68*P(1,4) - HK69*P(3,4) + HK73;
-const float HK77 = HK61*P(2,23) - HK61*P(2,5) + HK64*P(2,6) + HK65*P(2,22) - HK65*P(2,4) + HK66*P(2,2) + HK67*P(0,2) + HK68*P(1,2) - HK69*P(2,3);
-const float HK78 = HK61*P(5,23);
-const float HK79 = HK61*P(23,23) + HK64*P(6,23) + HK65*P(22,23) - HK65*P(4,23) + HK66*P(2,23) + HK67*P(0,23) + HK68*P(1,23) - HK69*P(3,23) - HK78;
-const float HK80 = HK61*HK71;
-const float HK81 = -HK61*P(5,5) + HK64*P(5,6) - HK65*P(4,5) + HK65*P(5,22) + HK66*P(2,5) + HK67*P(0,5) + HK68*P(1,5) - HK69*P(3,5) + HK78;
-const float HK82 = HK61*P(1,23) - HK61*P(1,5) + HK64*P(1,6) + HK65*P(1,22) - HK65*P(1,4) + HK66*P(1,2) + HK67*P(0,1) + HK68*P(1,1) - HK69*P(1,3);
-const float HK83 = HK61*P(3,23) - HK61*P(3,5) + HK64*P(3,6) + HK65*P(3,22) - HK65*P(3,4) + HK66*P(2,3) + HK67*P(0,3) + HK68*P(1,3) - HK69*P(3,3);
-const float HK84 = Kaccy/(HK64*HK71*HK72 + HK66*HK71*HK77 + HK67*HK70*HK71 + HK68*HK71*HK82 - HK69*HK71*HK83 + HK74*HK75 - HK75*HK76 + HK79*HK80 - HK80*HK81 + R_ACC);
+const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+const float HK3 = 2*Kaccx;
+const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
+const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
+const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
+const float HK7 = powf(q1, 2);
+const float HK8 = powf(q2, 2);
+const float HK9 = powf(q0, 2) - powf(q3, 2);
+const float HK10 = HK7 - HK8 + HK9;
+const float HK11 = HK10*Kaccx;
+const float HK12 = q0*q3;
+const float HK13 = q1*q2;
+const float HK14 = HK12 + HK13;
+const float HK15 = HK14*HK3;
+const float HK16 = q0*q2 - q1*q3;
+const float HK17 = 2*HK14;
+const float HK18 = 2*HK16;
+const float HK19 = 2*HK4;
+const float HK20 = 2*HK2;
+const float HK21 = 2*HK5;
+const float HK22 = 2*HK6;
+const float HK23 = HK22*P(0,3);
+const float HK24 = -HK10*P(0,22) + HK10*P(0,4) - HK17*P(0,23) + HK17*P(0,5) - HK18*P(0,6) + HK19*P(0,1) + HK20*P(0,0) - HK21*P(0,2) + HK23;
+const float HK25 = HK17*P(5,23);
+const float HK26 = -HK10*P(22,23) + HK10*P(4,23) - HK17*P(23,23) - HK18*P(6,23) + HK19*P(1,23) + HK20*P(0,23) - HK21*P(2,23) + HK22*P(3,23) + HK25;
+const float HK27 = powf(Kaccx, 2);
+const float HK28 = HK17*HK27;
+const float HK29 = HK10*P(4,5) - HK10*P(5,22) + HK17*P(5,5) - HK18*P(5,6) + HK19*P(1,5) + HK20*P(0,5) - HK21*P(2,5) + HK22*P(3,5) - HK25;
+const float HK30 = HK10*P(4,6) - HK10*P(6,22) + HK17*P(5,6) - HK17*P(6,23) - HK18*P(6,6) + HK19*P(1,6) + HK20*P(0,6) - HK21*P(2,6) + HK22*P(3,6);
+const float HK31 = HK10*P(4,22);
+const float HK32 = HK10*P(4,4) - HK17*P(4,23) + HK17*P(4,5) - HK18*P(4,6) + HK19*P(1,4) + HK20*P(0,4) - HK21*P(2,4) + HK22*P(3,4) - HK31;
+const float HK33 = HK10*HK27;
+const float HK34 = -HK10*P(22,22) - HK17*P(22,23) + HK17*P(5,22) - HK18*P(6,22) + HK19*P(1,22) + HK20*P(0,22) - HK21*P(2,22) + HK22*P(3,22) + HK31;
+const float HK35 = HK21*P(1,2);
+const float HK36 = -HK10*P(1,22) + HK10*P(1,4) - HK17*P(1,23) + HK17*P(1,5) - HK18*P(1,6) + HK19*P(1,1) + HK20*P(0,1) + HK22*P(1,3) - HK35;
+const float HK37 = HK19*P(1,2);
+const float HK38 = -HK10*P(2,22) + HK10*P(2,4) - HK17*P(2,23) + HK17*P(2,5) - HK18*P(2,6) + HK20*P(0,2) - HK21*P(2,2) + HK22*P(2,3) + HK37;
+const float HK39 = HK20*P(0,3);
+const float HK40 = -HK10*P(3,22) + HK10*P(3,4) - HK17*P(3,23) + HK17*P(3,5) - HK18*P(3,6) + HK19*P(1,3) - HK21*P(2,3) + HK22*P(3,3) + HK39;
+const float HK41 = Kaccx/(-HK18*HK27*HK30 + HK19*HK27*HK36 + HK20*HK24*HK27 - HK21*HK27*HK38 + HK22*HK27*HK40 - HK26*HK28 + HK28*HK29 + HK32*HK33 - HK33*HK34 + R_ACC);
+const float HK42 = HK12 - HK13;
+const float HK43 = 2*Kaccy;
+const float HK44 = HK42*HK43;
+const float HK45 = -HK7 + HK8 + HK9;
+const float HK46 = HK45*Kaccy;
+const float HK47 = q0*q1 + q2*q3;
+const float HK48 = 2*HK47;
+const float HK49 = 2*HK42;
+const float HK50 = HK19*P(0,2) + HK21*P(0,1) + HK22*P(0,0) - HK39 - HK45*P(0,23) + HK45*P(0,5) + HK48*P(0,6) + HK49*P(0,22) - HK49*P(0,4);
+const float HK51 = powf(Kaccy, 2);
+const float HK52 = HK19*P(2,6) - HK20*P(3,6) + HK21*P(1,6) + HK22*P(0,6) + HK45*P(5,6) - HK45*P(6,23) + HK48*P(6,6) - HK49*P(4,6) + HK49*P(6,22);
+const float HK53 = HK49*P(4,22);
+const float HK54 = HK19*P(2,22) - HK20*P(3,22) + HK21*P(1,22) + HK22*P(0,22) - HK45*P(22,23) + HK45*P(5,22) + HK48*P(6,22) + HK49*P(22,22) - HK53;
+const float HK55 = HK49*HK51;
+const float HK56 = HK19*P(2,4) - HK20*P(3,4) + HK21*P(1,4) + HK22*P(0,4) - HK45*P(4,23) + HK45*P(4,5) + HK48*P(4,6) - HK49*P(4,4) + HK53;
+const float HK57 = HK45*P(5,23);
+const float HK58 = HK19*P(2,5) - HK20*P(3,5) + HK21*P(1,5) + HK22*P(0,5) + HK45*P(5,5) + HK48*P(5,6) - HK49*P(4,5) + HK49*P(5,22) - HK57;
+const float HK59 = HK45*HK51;
+const float HK60 = HK19*P(2,23) - HK20*P(3,23) + HK21*P(1,23) + HK22*P(0,23) - HK45*P(23,23) + HK48*P(6,23) + HK49*P(22,23) - HK49*P(4,23) + HK57;
+const float HK61 = HK19*P(2,2) - HK20*P(2,3) + HK22*P(0,2) + HK35 - HK45*P(2,23) + HK45*P(2,5) + HK48*P(2,6) + HK49*P(2,22) - HK49*P(2,4);
+const float HK62 = -HK20*P(1,3) + HK21*P(1,1) + HK22*P(0,1) + HK37 - HK45*P(1,23) + HK45*P(1,5) + HK48*P(1,6) + HK49*P(1,22) - HK49*P(1,4);
+const float HK63 = HK19*P(2,3) - HK20*P(3,3) + HK21*P(1,3) + HK23 - HK45*P(3,23) + HK45*P(3,5) + HK48*P(3,6) + HK49*P(3,22) - HK49*P(3,4);
+const float HK64 = Kaccy/(HK19*HK51*HK61 - HK20*HK51*HK63 + HK21*HK51*HK62 + HK22*HK50*HK51 + HK48*HK51*HK52 + HK54*HK55 - HK55*HK56 + HK58*HK59 - HK59*HK60 + R_ACC);
 
 
 // Observation Jacobians - axis 0
-Hfusion.at<0>() = HK3*HK4;
-Hfusion.at<1>() = -HK4*HK6;
-Hfusion.at<2>() = HK12*HK4;
-Hfusion.at<3>() = -HK15*HK4;
-Hfusion.at<4>() = HK20;
-Hfusion.at<5>() = -HK24;
-Hfusion.at<6>() = HK27*HK4;
+Hfusion.at<0>() = -HK2*HK3;
+Hfusion.at<1>() = -HK3*HK4;
+Hfusion.at<2>() = HK3*HK5;
+Hfusion.at<3>() = -HK3*HK6;
+Hfusion.at<4>() = -HK11;
+Hfusion.at<5>() = -HK15;
+Hfusion.at<6>() = HK16*HK3;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -109,45 +89,47 @@ Hfusion.at<18>() = 0;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
-Hfusion.at<22>() = -HK20;
-Hfusion.at<23>() = HK24;
+Hfusion.at<22>() = HK11;
+Hfusion.at<23>() = HK15;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 0
-Kfusion(0) = HK53*(-HK19*P(0,4) - HK28*P(0,0) - HK29*P(0,2) - HK30*P(0,6) + HK34);
-Kfusion(1) = HK53*(-HK19*P(1,4) - HK28*P(0,1) - HK29*P(1,2) - HK30*P(1,6) + HK46);
-Kfusion(2) = HK53*(-HK19*P(2,4) - HK28*P(0,2) - HK29*P(2,2) - HK30*P(2,6) + HK52);
-Kfusion(3) = HK53*(-HK19*P(3,4) - HK28*P(0,3) - HK29*P(2,3) - HK30*P(3,6) + HK51);
-Kfusion(4) = HK53*(-HK19*P(4,4) - HK28*P(0,4) - HK29*P(2,4) - HK30*P(4,6) + HK48);
-Kfusion(5) = HK53*(-HK19*P(4,5) - HK28*P(0,5) - HK29*P(2,5) - HK30*P(5,6) + HK44);
-Kfusion(6) = HK53*(-HK19*P(4,6) - HK28*P(0,6) - HK29*P(2,6) - HK30*P(6,6) + HK45);
-Kfusion(7) = HK53*(-HK19*P(4,7) + HK19*P(7,22) - HK28*P(0,7) - HK29*P(2,7) - HK30*P(6,7) + HK31*P(5,7) - HK31*P(7,23) + HK32*P(1,7) + HK33*P(3,7));
-Kfusion(8) = HK53*(-HK19*P(4,8) + HK19*P(8,22) - HK28*P(0,8) - HK29*P(2,8) - HK30*P(6,8) + HK31*P(5,8) - HK31*P(8,23) + HK32*P(1,8) + HK33*P(3,8));
-Kfusion(9) = HK53*(-HK19*P(4,9) + HK19*P(9,22) - HK28*P(0,9) - HK29*P(2,9) - HK30*P(6,9) + HK31*P(5,9) - HK31*P(9,23) + HK32*P(1,9) + HK33*P(3,9));
-Kfusion(10) = HK53*(HK19*P(10,22) - HK19*P(4,10) - HK28*P(0,10) - HK29*P(2,10) - HK30*P(6,10) - HK31*P(10,23) + HK31*P(5,10) + HK32*P(1,10) + HK33*P(3,10));
-Kfusion(11) = HK53*(HK19*P(11,22) - HK19*P(4,11) - HK28*P(0,11) - HK29*P(2,11) - HK30*P(6,11) - HK31*P(11,23) + HK31*P(5,11) + HK32*P(1,11) + HK33*P(3,11));
-Kfusion(12) = HK53*(HK19*P(12,22) - HK19*P(4,12) - HK28*P(0,12) - HK29*P(2,12) - HK30*P(6,12) - HK31*P(12,23) + HK31*P(5,12) + HK32*P(1,12) + HK33*P(3,12));
-Kfusion(13) = HK53*(HK19*P(13,22) - HK19*P(4,13) - HK28*P(0,13) - HK29*P(2,13) - HK30*P(6,13) - HK31*P(13,23) + HK31*P(5,13) + HK32*P(1,13) + HK33*P(3,13));
-Kfusion(14) = HK53*(HK19*P(14,22) - HK19*P(4,14) - HK28*P(0,14) - HK29*P(2,14) - HK30*P(6,14) - HK31*P(14,23) + HK31*P(5,14) + HK32*P(1,14) + HK33*P(3,14));
-Kfusion(15) = HK53*(HK19*P(15,22) - HK19*P(4,15) - HK28*P(0,15) - HK29*P(2,15) - HK30*P(6,15) - HK31*P(15,23) + HK31*P(5,15) + HK32*P(1,15) + HK33*P(3,15));
-Kfusion(16) = HK53*(HK19*P(16,22) - HK19*P(4,16) - HK28*P(0,16) - HK29*P(2,16) - HK30*P(6,16) - HK31*P(16,23) + HK31*P(5,16) + HK32*P(1,16) + HK33*P(3,16));
-Kfusion(17) = HK53*(HK19*P(17,22) - HK19*P(4,17) - HK28*P(0,17) - HK29*P(2,17) - HK30*P(6,17) - HK31*P(17,23) + HK31*P(5,17) + HK32*P(1,17) + HK33*P(3,17));
-Kfusion(18) = HK53*(HK19*P(18,22) - HK19*P(4,18) - HK28*P(0,18) - HK29*P(2,18) - HK30*P(6,18) - HK31*P(18,23) + HK31*P(5,18) + HK32*P(1,18) + HK33*P(3,18));
-Kfusion(19) = HK53*(HK19*P(19,22) - HK19*P(4,19) - HK28*P(0,19) - HK29*P(2,19) - HK30*P(6,19) - HK31*P(19,23) + HK31*P(5,19) + HK32*P(1,19) + HK33*P(3,19));
-Kfusion(20) = HK53*(HK19*P(20,22) - HK19*P(4,20) - HK28*P(0,20) - HK29*P(2,20) - HK30*P(6,20) - HK31*P(20,23) + HK31*P(5,20) + HK32*P(1,20) + HK33*P(3,20));
-Kfusion(21) = HK53*(HK19*P(21,22) - HK19*P(4,21) - HK28*P(0,21) - HK29*P(2,21) - HK30*P(6,21) - HK31*P(21,23) + HK31*P(5,21) + HK32*P(1,21) + HK33*P(3,21));
-Kfusion(22) = HK53*(-HK28*P(0,22) - HK29*P(2,22) - HK30*P(6,22) - HK47 + HK50);
-Kfusion(23) = HK53*(-HK19*P(4,23) - HK28*P(0,23) - HK29*P(2,23) - HK30*P(6,23) + HK41);
+Kfusion(0) = -HK24*HK41;
+Kfusion(1) = -HK36*HK41;
+Kfusion(2) = -HK38*HK41;
+Kfusion(3) = -HK40*HK41;
+Kfusion(4) = -HK32*HK41;
+Kfusion(5) = -HK29*HK41;
+Kfusion(6) = -HK30*HK41;
+Kfusion(7) = -HK41*(HK10*P(4,7) - HK10*P(7,22) + HK17*P(5,7) - HK17*P(7,23) - HK18*P(6,7) + HK19*P(1,7) + HK20*P(0,7) - HK21*P(2,7) + HK22*P(3,7));
+Kfusion(8) = -HK41*(HK10*P(4,8) - HK10*P(8,22) + HK17*P(5,8) - HK17*P(8,23) - HK18*P(6,8) + HK19*P(1,8) + HK20*P(0,8) - HK21*P(2,8) + HK22*P(3,8));
+Kfusion(9) = -HK41*(HK10*P(4,9) - HK10*P(9,22) + HK17*P(5,9) - HK17*P(9,23) - HK18*P(6,9) + HK19*P(1,9) + HK20*P(0,9) - HK21*P(2,9) + HK22*P(3,9));
+Kfusion(10) = -HK41*(-HK10*P(10,22) + HK10*P(4,10) - HK17*P(10,23) + HK17*P(5,10) - HK18*P(6,10) + HK19*P(1,10) + HK20*P(0,10) - HK21*P(2,10) + HK22*P(3,10));
+Kfusion(11) = -HK41*(-HK10*P(11,22) + HK10*P(4,11) - HK17*P(11,23) + HK17*P(5,11) - HK18*P(6,11) + HK19*P(1,11) + HK20*P(0,11) - HK21*P(2,11) + HK22*P(3,11));
+Kfusion(12) = -HK41*(-HK10*P(12,22) + HK10*P(4,12) - HK17*P(12,23) + HK17*P(5,12) - HK18*P(6,12) + HK19*P(1,12) + HK20*P(0,12) - HK21*P(2,12) + HK22*P(3,12));
+Kfusion(13) = -HK41*(-HK10*P(13,22) + HK10*P(4,13) - HK17*P(13,23) + HK17*P(5,13) - HK18*P(6,13) + HK19*P(1,13) + HK20*P(0,13) - HK21*P(2,13) + HK22*P(3,13));
+Kfusion(14) = -HK41*(-HK10*P(14,22) + HK10*P(4,14) - HK17*P(14,23) + HK17*P(5,14) - HK18*P(6,14) + HK19*P(1,14) + HK20*P(0,14) - HK21*P(2,14) + HK22*P(3,14));
+Kfusion(15) = -HK41*(-HK10*P(15,22) + HK10*P(4,15) - HK17*P(15,23) + HK17*P(5,15) - HK18*P(6,15) + HK19*P(1,15) + HK20*P(0,15) - HK21*P(2,15) + HK22*P(3,15));
+Kfusion(16) = -HK41*(-HK10*P(16,22) + HK10*P(4,16) - HK17*P(16,23) + HK17*P(5,16) - HK18*P(6,16) + HK19*P(1,16) + HK20*P(0,16) - HK21*P(2,16) + HK22*P(3,16));
+Kfusion(17) = -HK41*(-HK10*P(17,22) + HK10*P(4,17) - HK17*P(17,23) + HK17*P(5,17) - HK18*P(6,17) + HK19*P(1,17) + HK20*P(0,17) - HK21*P(2,17) + HK22*P(3,17));
+Kfusion(18) = -HK41*(-HK10*P(18,22) + HK10*P(4,18) - HK17*P(18,23) + HK17*P(5,18) - HK18*P(6,18) + HK19*P(1,18) + HK20*P(0,18) - HK21*P(2,18) + HK22*P(3,18));
+Kfusion(19) = -HK41*(-HK10*P(19,22) + HK10*P(4,19) - HK17*P(19,23) + HK17*P(5,19) - HK18*P(6,19) + HK19*P(1,19) + HK20*P(0,19) - HK21*P(2,19) + HK22*P(3,19));
+Kfusion(20) = -HK41*(-HK10*P(20,22) + HK10*P(4,20) - HK17*P(20,23) + HK17*P(5,20) - HK18*P(6,20) + HK19*P(1,20) + HK20*P(0,20) - HK21*P(2,20) + HK22*P(3,20));
+Kfusion(21) = -HK41*(-HK10*P(21,22) + HK10*P(4,21) - HK17*P(21,23) + HK17*P(5,21) - HK18*P(6,21) + HK19*P(1,21) + HK20*P(0,21) - HK21*P(2,21) + HK22*P(3,21));
+Kfusion(22) = -HK34*HK41;
+Kfusion(23) = -HK26*HK41;
+Kfusion(24) = -HK41*(-HK10*P(22,24) + HK10*P(4,24) - HK17*P(23,24) + HK17*P(5,24) - HK18*P(6,24) + HK19*P(1,24) + HK20*P(0,24) - HK21*P(2,24) + HK22*P(3,24));
 
 
 // Observation Jacobians - axis 1
-Hfusion.at<0>() = -HK54*HK55;
-Hfusion.at<1>() = -HK55*HK56;
-Hfusion.at<2>() = -HK55*HK57;
-Hfusion.at<3>() = HK55*HK58;
-Hfusion.at<4>() = HK60;
-Hfusion.at<5>() = HK62;
-Hfusion.at<6>() = -HK55*HK63;
+Hfusion.at<0>() = -HK22*Kaccy;
+Hfusion.at<1>() = -HK21*Kaccy;
+Hfusion.at<2>() = -HK19*Kaccy;
+Hfusion.at<3>() = HK20*Kaccy;
+Hfusion.at<4>() = HK44;
+Hfusion.at<5>() = -HK46;
+Hfusion.at<6>() = -HK43*HK47;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -163,35 +145,37 @@ Hfusion.at<18>() = 0;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
-Hfusion.at<22>() = -HK60;
-Hfusion.at<23>() = -HK62;
+Hfusion.at<22>() = -HK44;
+Hfusion.at<23>() = HK46;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 1
-Kfusion(0) = -HK70*HK84;
-Kfusion(1) = -HK82*HK84;
-Kfusion(2) = -HK77*HK84;
-Kfusion(3) = -HK83*HK84;
-Kfusion(4) = -HK76*HK84;
-Kfusion(5) = -HK81*HK84;
-Kfusion(6) = -HK72*HK84;
-Kfusion(7) = -HK84*(-HK61*P(5,7) + HK61*P(7,23) + HK64*P(6,7) - HK65*P(4,7) + HK65*P(7,22) + HK66*P(2,7) + HK67*P(0,7) + HK68*P(1,7) - HK69*P(3,7));
-Kfusion(8) = -HK84*(-HK61*P(5,8) + HK61*P(8,23) + HK64*P(6,8) - HK65*P(4,8) + HK65*P(8,22) + HK66*P(2,8) + HK67*P(0,8) + HK68*P(1,8) - HK69*P(3,8));
-Kfusion(9) = -HK84*(-HK61*P(5,9) + HK61*P(9,23) + HK64*P(6,9) - HK65*P(4,9) + HK65*P(9,22) + HK66*P(2,9) + HK67*P(0,9) + HK68*P(1,9) - HK69*P(3,9));
-Kfusion(10) = -HK84*(HK61*P(10,23) - HK61*P(5,10) + HK64*P(6,10) + HK65*P(10,22) - HK65*P(4,10) + HK66*P(2,10) + HK67*P(0,10) + HK68*P(1,10) - HK69*P(3,10));
-Kfusion(11) = -HK84*(HK61*P(11,23) - HK61*P(5,11) + HK64*P(6,11) + HK65*P(11,22) - HK65*P(4,11) + HK66*P(2,11) + HK67*P(0,11) + HK68*P(1,11) - HK69*P(3,11));
-Kfusion(12) = -HK84*(HK61*P(12,23) - HK61*P(5,12) + HK64*P(6,12) + HK65*P(12,22) - HK65*P(4,12) + HK66*P(2,12) + HK67*P(0,12) + HK68*P(1,12) - HK69*P(3,12));
-Kfusion(13) = -HK84*(HK61*P(13,23) - HK61*P(5,13) + HK64*P(6,13) + HK65*P(13,22) - HK65*P(4,13) + HK66*P(2,13) + HK67*P(0,13) + HK68*P(1,13) - HK69*P(3,13));
-Kfusion(14) = -HK84*(HK61*P(14,23) - HK61*P(5,14) + HK64*P(6,14) + HK65*P(14,22) - HK65*P(4,14) + HK66*P(2,14) + HK67*P(0,14) + HK68*P(1,14) - HK69*P(3,14));
-Kfusion(15) = -HK84*(HK61*P(15,23) - HK61*P(5,15) + HK64*P(6,15) + HK65*P(15,22) - HK65*P(4,15) + HK66*P(2,15) + HK67*P(0,15) + HK68*P(1,15) - HK69*P(3,15));
-Kfusion(16) = -HK84*(HK61*P(16,23) - HK61*P(5,16) + HK64*P(6,16) + HK65*P(16,22) - HK65*P(4,16) + HK66*P(2,16) + HK67*P(0,16) + HK68*P(1,16) - HK69*P(3,16));
-Kfusion(17) = -HK84*(HK61*P(17,23) - HK61*P(5,17) + HK64*P(6,17) + HK65*P(17,22) - HK65*P(4,17) + HK66*P(2,17) + HK67*P(0,17) + HK68*P(1,17) - HK69*P(3,17));
-Kfusion(18) = -HK84*(HK61*P(18,23) - HK61*P(5,18) + HK64*P(6,18) + HK65*P(18,22) - HK65*P(4,18) + HK66*P(2,18) + HK67*P(0,18) + HK68*P(1,18) - HK69*P(3,18));
-Kfusion(19) = -HK84*(HK61*P(19,23) - HK61*P(5,19) + HK64*P(6,19) + HK65*P(19,22) - HK65*P(4,19) + HK66*P(2,19) + HK67*P(0,19) + HK68*P(1,19) - HK69*P(3,19));
-Kfusion(20) = -HK84*(HK61*P(20,23) - HK61*P(5,20) + HK64*P(6,20) + HK65*P(20,22) - HK65*P(4,20) + HK66*P(2,20) + HK67*P(0,20) + HK68*P(1,20) - HK69*P(3,20));
-Kfusion(21) = -HK84*(HK61*P(21,23) - HK61*P(5,21) + HK64*P(6,21) + HK65*P(21,22) - HK65*P(4,21) + HK66*P(2,21) + HK67*P(0,21) + HK68*P(1,21) - HK69*P(3,21));
-Kfusion(22) = -HK74*HK84;
-Kfusion(23) = -HK79*HK84;
+Kfusion(0) = -HK50*HK64;
+Kfusion(1) = -HK62*HK64;
+Kfusion(2) = -HK61*HK64;
+Kfusion(3) = -HK63*HK64;
+Kfusion(4) = -HK56*HK64;
+Kfusion(5) = -HK58*HK64;
+Kfusion(6) = -HK52*HK64;
+Kfusion(7) = -HK64*(HK19*P(2,7) - HK20*P(3,7) + HK21*P(1,7) + HK22*P(0,7) + HK45*P(5,7) - HK45*P(7,23) + HK48*P(6,7) - HK49*P(4,7) + HK49*P(7,22));
+Kfusion(8) = -HK64*(HK19*P(2,8) - HK20*P(3,8) + HK21*P(1,8) + HK22*P(0,8) + HK45*P(5,8) - HK45*P(8,23) + HK48*P(6,8) - HK49*P(4,8) + HK49*P(8,22));
+Kfusion(9) = -HK64*(HK19*P(2,9) - HK20*P(3,9) + HK21*P(1,9) + HK22*P(0,9) + HK45*P(5,9) - HK45*P(9,23) + HK48*P(6,9) - HK49*P(4,9) + HK49*P(9,22));
+Kfusion(10) = -HK64*(HK19*P(2,10) - HK20*P(3,10) + HK21*P(1,10) + HK22*P(0,10) - HK45*P(10,23) + HK45*P(5,10) + HK48*P(6,10) + HK49*P(10,22) - HK49*P(4,10));
+Kfusion(11) = -HK64*(HK19*P(2,11) - HK20*P(3,11) + HK21*P(1,11) + HK22*P(0,11) - HK45*P(11,23) + HK45*P(5,11) + HK48*P(6,11) + HK49*P(11,22) - HK49*P(4,11));
+Kfusion(12) = -HK64*(HK19*P(2,12) - HK20*P(3,12) + HK21*P(1,12) + HK22*P(0,12) - HK45*P(12,23) + HK45*P(5,12) + HK48*P(6,12) + HK49*P(12,22) - HK49*P(4,12));
+Kfusion(13) = -HK64*(HK19*P(2,13) - HK20*P(3,13) + HK21*P(1,13) + HK22*P(0,13) - HK45*P(13,23) + HK45*P(5,13) + HK48*P(6,13) + HK49*P(13,22) - HK49*P(4,13));
+Kfusion(14) = -HK64*(HK19*P(2,14) - HK20*P(3,14) + HK21*P(1,14) + HK22*P(0,14) - HK45*P(14,23) + HK45*P(5,14) + HK48*P(6,14) + HK49*P(14,22) - HK49*P(4,14));
+Kfusion(15) = -HK64*(HK19*P(2,15) - HK20*P(3,15) + HK21*P(1,15) + HK22*P(0,15) - HK45*P(15,23) + HK45*P(5,15) + HK48*P(6,15) + HK49*P(15,22) - HK49*P(4,15));
+Kfusion(16) = -HK64*(HK19*P(2,16) - HK20*P(3,16) + HK21*P(1,16) + HK22*P(0,16) - HK45*P(16,23) + HK45*P(5,16) + HK48*P(6,16) + HK49*P(16,22) - HK49*P(4,16));
+Kfusion(17) = -HK64*(HK19*P(2,17) - HK20*P(3,17) + HK21*P(1,17) + HK22*P(0,17) - HK45*P(17,23) + HK45*P(5,17) + HK48*P(6,17) + HK49*P(17,22) - HK49*P(4,17));
+Kfusion(18) = -HK64*(HK19*P(2,18) - HK20*P(3,18) + HK21*P(1,18) + HK22*P(0,18) - HK45*P(18,23) + HK45*P(5,18) + HK48*P(6,18) + HK49*P(18,22) - HK49*P(4,18));
+Kfusion(19) = -HK64*(HK19*P(2,19) - HK20*P(3,19) + HK21*P(1,19) + HK22*P(0,19) - HK45*P(19,23) + HK45*P(5,19) + HK48*P(6,19) + HK49*P(19,22) - HK49*P(4,19));
+Kfusion(20) = -HK64*(HK19*P(2,20) - HK20*P(3,20) + HK21*P(1,20) + HK22*P(0,20) - HK45*P(20,23) + HK45*P(5,20) + HK48*P(6,20) + HK49*P(20,22) - HK49*P(4,20));
+Kfusion(21) = -HK64*(HK19*P(2,21) - HK20*P(3,21) + HK21*P(1,21) + HK22*P(0,21) - HK45*P(21,23) + HK45*P(5,21) + HK48*P(6,21) + HK49*P(21,22) - HK49*P(4,21));
+Kfusion(22) = -HK54*HK64;
+Kfusion(23) = -HK60*HK64;
+Kfusion(24) = -HK64*(HK19*P(2,24) - HK20*P(3,24) + HK21*P(1,24) + HK22*P(0,24) - HK45*P(23,24) + HK45*P(5,24) + HK48*P(6,24) + HK49*P(22,24) - HK49*P(4,24));
 
 
 // Observation Jacobians - axis 2

src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp

@@ -4,19 +4,19 @@
 #include "../../../../../matrix/matrix/math.hpp"
 #include "util.h"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
 
 int main()
 {
     // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
+    Vector25f Hfusion_sympy;
+    Vector25f Kfusion_sympy;
 
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
+    Vector25f Hfusion_matlab;
+    Vector25f Kfusion_matlab;
 
     float _beta_innov_var;
 
@@ -43,7 +43,7 @@ int main()
 	const float vwe = 5.0f * 2.0f * ((float)rand() - 0.5f);
 
     // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {
@@ -157,7 +157,7 @@ int main()
         // }
 
         // Observation Jacobians
-        SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
+        SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
         Hfusion.at<0>() = HK21*HK22;
         Hfusion.at<1>() = HK22*HK25;
         Hfusion.at<2>() = HK22*HK26;
@@ -169,7 +169,7 @@ int main()
         Hfusion.at<23>() = HK16*HK32;
 
         // Calculate Kalman gains
-        Vector24f Kfusion;
+        Vector25f Kfusion;
         if (!update_wind_only) {
 
             Kfusion(0) = HK38*HK52;
@@ -224,7 +224,7 @@ int main()
         SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd;
         SH_BETA[12] = 2.0f*q0*q3;
 
-        Vector24f H_BETA;
+        Vector25f H_BETA;
         H_BETA(0) = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
         H_BETA(1) = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
         H_BETA(2) = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
@@ -248,7 +248,7 @@ int main()
         SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
         SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
 
-        Vector24f Kfusion;
+        Vector25f Kfusion;
         Kfusion(0) = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]);
         Kfusion(1) = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]);
         Kfusion(2) = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]);

src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp

@@ -1,70 +1,67 @@
 // Sub Expressions
-const float HK0 = q1*vd;
-const float HK1 = vn - vwn;
-const float HK2 = HK1*q3;
-const float HK3 = q2*vd;
-const float HK4 = ve - vwe;
-const float HK5 = HK4*q3;
-const float HK6 = q0*q2 - q1*q3;
-const float HK7 = 2*vd;
-const float HK8 = HK6*HK7;
-const float HK9 = q0*q3;
-const float HK10 = q1*q2;
-const float HK11 = 2*HK10 + 2*HK9;
-const float HK12 = HK11*HK4;
-const float HK13 = 2*powf(q3, 2) - 1;
-const float HK14 = HK13 + 2*powf(q2, 2);
-const float HK15 = HK1*HK14;
-const float HK16 = 1.0F/(-HK12 + HK15 + HK8);
+const float HK0 = vn - vwn;
+const float HK1 = ve - vwe;
+const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+const float HK3 = q0*q2 - q1*q3;
+const float HK4 = 2*vd;
+const float HK5 = q0*q3;
+const float HK6 = q1*q2;
+const float HK7 = 2*HK5 + 2*HK6;
+const float HK8 = powf(q0, 2);
+const float HK9 = powf(q3, 2);
+const float HK10 = HK8 - HK9;
+const float HK11 = powf(q1, 2);
+const float HK12 = powf(q2, 2);
+const float HK13 = HK11 - HK12;
+const float HK14 = HK10 + HK13;
+const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4;
+const float HK16 = 1.0F/HK15;
 const float HK17 = q0*q1 + q2*q3;
-const float HK18 = HK17*HK7;
-const float HK19 = 2*HK1*(-HK10 + HK9);
-const float HK20 = HK13 + 2*powf(q1, 2);
-const float HK21 = HK20*HK4;
-const float HK22 = HK16*(-HK18 + HK19 + HK21);
-const float HK23 = 2*HK16;
-const float HK24 = HK23*(HK0 - HK2 + HK22*(HK3 - HK5));
-const float HK25 = q0*vd;
-const float HK26 = HK1*q2;
-const float HK27 = HK4*q1;
-const float HK28 = 2*HK27;
-const float HK29 = q3*vd;
-const float HK30 = 1.0F/(HK12 - HK15 - HK8);
-const float HK31 = HK30*(HK18 - HK19 - HK21);
-const float HK32 = HK31*(HK29 + HK4*q2);
-const float HK33 = 2*HK30;
-const float HK34 = HK23*(HK1*q1 + HK22*(HK25 + 2*HK26 - HK27) + HK29);
-const float HK35 = HK33*(HK1*q0 - HK3 + HK31*(HK0 - 2*HK2 + HK4*q0) + 2*HK5);
-const float HK36 = HK14*HK31;
-const float HK37 = 2*HK9;
-const float HK38 = 2*HK10;
-const float HK39 = -HK37 + HK38;
-const float HK40 = HK30*(HK11*HK31 + HK20);
-const float HK41 = HK23*(HK17 + HK22*HK6);
-const float HK42 = HK16*(HK14*HK22 + HK39);
-const float HK43 = HK16*(HK11*HK22 + HK20);
-const float HK44 = HK30*(-HK36 + HK37 - HK38);
-const float HK45 = HK33*(-HK25 - HK26 + HK28 + HK32);
-const float HK46 = -HK24*P(0,0) - HK34*P(0,2) - HK35*P(0,3) - HK40*P(0,5) - HK41*P(0,6) + HK42*P(0,22) - HK43*P(0,23) - HK44*P(0,4) - HK45*P(0,1);
-const float HK47 = -HK24*P(0,6) - HK34*P(2,6) - HK35*P(3,6) - HK40*P(5,6) - HK41*P(6,6) + HK42*P(6,22) - HK43*P(6,23) - HK44*P(4,6) - HK45*P(1,6);
-const float HK48 = -HK24*P(0,22) - HK34*P(2,22) - HK35*P(3,22) - HK40*P(5,22) - HK41*P(6,22) + HK42*P(22,22) - HK43*P(22,23) - HK44*P(4,22) - HK45*P(1,22);
-const float HK49 = -HK24*P(0,23) - HK34*P(2,23) - HK35*P(3,23) - HK40*P(5,23) - HK41*P(6,23) + HK42*P(22,23) - HK43*P(23,23) - HK44*P(4,23) - HK45*P(1,23);
-const float HK50 = -HK24*P(0,5) - HK34*P(2,5) - HK35*P(3,5) - HK40*P(5,5) - HK41*P(5,6) + HK42*P(5,22) - HK43*P(5,23) - HK44*P(4,5) - HK45*P(1,5);
-const float HK51 = -HK24*P(0,4) - HK34*P(2,4) - HK35*P(3,4) - HK40*P(4,5) - HK41*P(4,6) + HK42*P(4,22) - HK43*P(4,23) - HK44*P(4,4) - HK45*P(1,4);
-const float HK52 = -HK24*P(0,2) - HK34*P(2,2) - HK35*P(2,3) - HK40*P(2,5) - HK41*P(2,6) + HK42*P(2,22) - HK43*P(2,23) - HK44*P(2,4) - HK45*P(1,2);
-const float HK53 = -HK24*P(0,1) - HK34*P(1,2) - HK35*P(1,3) - HK40*P(1,5) - HK41*P(1,6) + HK42*P(1,22) - HK43*P(1,23) - HK44*P(1,4) - HK45*P(1,1);
-const float HK54 = -HK24*P(0,3) - HK34*P(2,3) - HK35*P(3,3) - HK40*P(3,5) - HK41*P(3,6) + HK42*P(3,22) - HK43*P(3,23) - HK44*P(3,4) - HK45*P(1,3);
-const float HK55 = 1.0F/(-HK24*HK46 - HK34*HK52 - HK35*HK54 - HK40*HK50 - HK41*HK47 + HK42*HK48 - HK43*HK49 - HK44*HK51 - HK45*HK53 + R_BETA);
+const float HK18 = HK10 - HK11 + HK12;
+const float HK19 = HK16*(-2*HK0*(HK5 - HK6) + HK1*HK18 + HK17*HK4);
+const float HK20 = -HK0*q3 + HK1*q0 + q1*vd;
+const float HK21 = -HK19*HK2 + HK20;
+const float HK22 = 2*HK16;
+const float HK23 = HK0*q1 + HK1*q2 + q3*vd;
+const float HK24 = HK0*q2 - HK1*q1 + q0*vd;
+const float HK25 = -HK19*HK23 + HK24;
+const float HK26 = HK19*HK24 + HK23;
+const float HK27 = HK19*HK20 + HK2;
+const float HK28 = HK14*HK19 + 2*HK5 - 2*HK6;
+const float HK29 = HK16*HK28;
+const float HK30 = HK19*HK7;
+const float HK31 = HK17 + HK19*HK3;
+const float HK32 = HK13 + HK30 - HK8 + HK9;
+const float HK33 = 2*HK31;
+const float HK34 = 2*HK26;
+const float HK35 = 2*HK25;
+const float HK36 = 2*HK27;
+const float HK37 = 2*HK21;
+const float HK38 = HK28*P(0,22) - HK28*P(0,4) + HK32*P(0,23) - HK32*P(0,5) + HK33*P(0,6) + HK34*P(0,2) + HK35*P(0,1) - HK36*P(0,3) + HK37*P(0,0);
+const float HK39 = powf(HK15, -2);
+const float HK40 = -HK28*P(4,6) + HK28*P(6,22) - HK32*P(5,6) + HK32*P(6,23) + HK33*P(6,6) + HK34*P(2,6) + HK35*P(1,6) - HK36*P(3,6) + HK37*P(0,6);
+const float HK41 = HK32*P(5,23);
+const float HK42 = HK28*P(22,23) - HK28*P(4,23) + HK32*P(23,23) + HK33*P(6,23) + HK34*P(2,23) + HK35*P(1,23) - HK36*P(3,23) + HK37*P(0,23) - HK41;
+const float HK43 = HK32*HK39;
+const float HK44 = HK28*P(4,22);
+const float HK45 = HK28*P(22,22) + HK32*P(22,23) - HK32*P(5,22) + HK33*P(6,22) + HK34*P(2,22) + HK35*P(1,22) - HK36*P(3,22) + HK37*P(0,22) - HK44;
+const float HK46 = HK28*HK39;
+const float HK47 = -HK28*P(4,5) + HK28*P(5,22) - HK32*P(5,5) + HK33*P(5,6) + HK34*P(2,5) + HK35*P(1,5) - HK36*P(3,5) + HK37*P(0,5) + HK41;
+const float HK48 = -HK28*P(4,4) + HK32*P(4,23) - HK32*P(4,5) + HK33*P(4,6) + HK34*P(2,4) + HK35*P(1,4) - HK36*P(3,4) + HK37*P(0,4) + HK44;
+const float HK49 = HK28*P(2,22) - HK28*P(2,4) + HK32*P(2,23) - HK32*P(2,5) + HK33*P(2,6) + HK34*P(2,2) + HK35*P(1,2) - HK36*P(2,3) + HK37*P(0,2);
+const float HK50 = HK28*P(1,22) - HK28*P(1,4) + HK32*P(1,23) - HK32*P(1,5) + HK33*P(1,6) + HK34*P(1,2) + HK35*P(1,1) - HK36*P(1,3) + HK37*P(0,1);
+const float HK51 = HK28*P(3,22) - HK28*P(3,4) + HK32*P(3,23) - HK32*P(3,5) + HK33*P(3,6) + HK34*P(2,3) + HK35*P(1,3) - HK36*P(3,3) + HK37*P(0,3);
+const float HK52 = HK16/(HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = -HK24;
-Hfusion.at<1>() = HK33*(HK25 + HK26 - HK28 - HK32);
-Hfusion.at<2>() = -HK34;
-Hfusion.at<3>() = -HK35;
-Hfusion.at<4>() = HK30*(HK36 + HK39);
-Hfusion.at<5>() = -HK40;
-Hfusion.at<6>() = -HK41;
+Hfusion.at<0>() = HK21*HK22;
+Hfusion.at<1>() = HK22*HK25;
+Hfusion.at<2>() = HK22*HK26;
+Hfusion.at<3>() = -HK22*HK27;
+Hfusion.at<4>() = -HK29;
+Hfusion.at<5>() = HK16*(HK18 - HK30);
+Hfusion.at<6>() = HK22*HK31;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -80,32 +77,34 @@ Hfusion.at<18>() = 0;
 Hfusion.at<19>() = 0;
 Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
-Hfusion.at<22>() = HK42;
-Hfusion.at<23>() = -HK43;
+Hfusion.at<22>() = HK29;
+Hfusion.at<23>() = HK16*HK32;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HK46*HK55;
-Kfusion(1) = HK53*HK55;
-Kfusion(2) = HK52*HK55;
-Kfusion(3) = HK54*HK55;
-Kfusion(4) = HK51*HK55;
-Kfusion(5) = HK50*HK55;
-Kfusion(6) = HK47*HK55;
-Kfusion(7) = HK55*(-HK24*P(0,7) - HK34*P(2,7) - HK35*P(3,7) - HK40*P(5,7) - HK41*P(6,7) + HK42*P(7,22) - HK43*P(7,23) - HK44*P(4,7) - HK45*P(1,7));
-Kfusion(8) = HK55*(-HK24*P(0,8) - HK34*P(2,8) - HK35*P(3,8) - HK40*P(5,8) - HK41*P(6,8) + HK42*P(8,22) - HK43*P(8,23) - HK44*P(4,8) - HK45*P(1,8));
-Kfusion(9) = HK55*(-HK24*P(0,9) - HK34*P(2,9) - HK35*P(3,9) - HK40*P(5,9) - HK41*P(6,9) + HK42*P(9,22) - HK43*P(9,23) - HK44*P(4,9) - HK45*P(1,9));
-Kfusion(10) = HK55*(-HK24*P(0,10) - HK34*P(2,10) - HK35*P(3,10) - HK40*P(5,10) - HK41*P(6,10) + HK42*P(10,22) - HK43*P(10,23) - HK44*P(4,10) - HK45*P(1,10));
-Kfusion(11) = HK55*(-HK24*P(0,11) - HK34*P(2,11) - HK35*P(3,11) - HK40*P(5,11) - HK41*P(6,11) + HK42*P(11,22) - HK43*P(11,23) - HK44*P(4,11) - HK45*P(1,11));
-Kfusion(12) = HK55*(-HK24*P(0,12) - HK34*P(2,12) - HK35*P(3,12) - HK40*P(5,12) - HK41*P(6,12) + HK42*P(12,22) - HK43*P(12,23) - HK44*P(4,12) - HK45*P(1,12));
-Kfusion(13) = HK55*(-HK24*P(0,13) - HK34*P(2,13) - HK35*P(3,13) - HK40*P(5,13) - HK41*P(6,13) + HK42*P(13,22) - HK43*P(13,23) - HK44*P(4,13) - HK45*P(1,13));
-Kfusion(14) = HK55*(-HK24*P(0,14) - HK34*P(2,14) - HK35*P(3,14) - HK40*P(5,14) - HK41*P(6,14) + HK42*P(14,22) - HK43*P(14,23) - HK44*P(4,14) - HK45*P(1,14));
-Kfusion(15) = HK55*(-HK24*P(0,15) - HK34*P(2,15) - HK35*P(3,15) - HK40*P(5,15) - HK41*P(6,15) + HK42*P(15,22) - HK43*P(15,23) - HK44*P(4,15) - HK45*P(1,15));
-Kfusion(16) = HK55*(-HK24*P(0,16) - HK34*P(2,16) - HK35*P(3,16) - HK40*P(5,16) - HK41*P(6,16) + HK42*P(16,22) - HK43*P(16,23) - HK44*P(4,16) - HK45*P(1,16));
-Kfusion(17) = HK55*(-HK24*P(0,17) - HK34*P(2,17) - HK35*P(3,17) - HK40*P(5,17) - HK41*P(6,17) + HK42*P(17,22) - HK43*P(17,23) - HK44*P(4,17) - HK45*P(1,17));
-Kfusion(18) = HK55*(-HK24*P(0,18) - HK34*P(2,18) - HK35*P(3,18) - HK40*P(5,18) - HK41*P(6,18) + HK42*P(18,22) - HK43*P(18,23) - HK44*P(4,18) - HK45*P(1,18));
-Kfusion(19) = HK55*(-HK24*P(0,19) - HK34*P(2,19) - HK35*P(3,19) - HK40*P(5,19) - HK41*P(6,19) + HK42*P(19,22) - HK43*P(19,23) - HK44*P(4,19) - HK45*P(1,19));
-Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) - HK41*P(6,20) + HK42*P(20,22) - HK43*P(20,23) - HK44*P(4,20) - HK45*P(1,20));
-Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21));
-Kfusion(22) = HK48*HK55;
-Kfusion(23) = HK49*HK55;
+Kfusion(0) = HK38*HK52;
+Kfusion(1) = HK50*HK52;
+Kfusion(2) = HK49*HK52;
+Kfusion(3) = HK51*HK52;
+Kfusion(4) = HK48*HK52;
+Kfusion(5) = HK47*HK52;
+Kfusion(6) = HK40*HK52;
+Kfusion(7) = HK52*(-HK28*P(4,7) + HK28*P(7,22) - HK32*P(5,7) + HK32*P(7,23) + HK33*P(6,7) + HK34*P(2,7) + HK35*P(1,7) - HK36*P(3,7) + HK37*P(0,7));
+Kfusion(8) = HK52*(-HK28*P(4,8) + HK28*P(8,22) - HK32*P(5,8) + HK32*P(8,23) + HK33*P(6,8) + HK34*P(2,8) + HK35*P(1,8) - HK36*P(3,8) + HK37*P(0,8));
+Kfusion(9) = HK52*(-HK28*P(4,9) + HK28*P(9,22) - HK32*P(5,9) + HK32*P(9,23) + HK33*P(6,9) + HK34*P(2,9) + HK35*P(1,9) - HK36*P(3,9) + HK37*P(0,9));
+Kfusion(10) = HK52*(HK28*P(10,22) - HK28*P(4,10) + HK32*P(10,23) - HK32*P(5,10) + HK33*P(6,10) + HK34*P(2,10) + HK35*P(1,10) - HK36*P(3,10) + HK37*P(0,10));
+Kfusion(11) = HK52*(HK28*P(11,22) - HK28*P(4,11) + HK32*P(11,23) - HK32*P(5,11) + HK33*P(6,11) + HK34*P(2,11) + HK35*P(1,11) - HK36*P(3,11) + HK37*P(0,11));
+Kfusion(12) = HK52*(HK28*P(12,22) - HK28*P(4,12) + HK32*P(12,23) - HK32*P(5,12) + HK33*P(6,12) + HK34*P(2,12) + HK35*P(1,12) - HK36*P(3,12) + HK37*P(0,12));
+Kfusion(13) = HK52*(HK28*P(13,22) - HK28*P(4,13) + HK32*P(13,23) - HK32*P(5,13) + HK33*P(6,13) + HK34*P(2,13) + HK35*P(1,13) - HK36*P(3,13) + HK37*P(0,13));
+Kfusion(14) = HK52*(HK28*P(14,22) - HK28*P(4,14) + HK32*P(14,23) - HK32*P(5,14) + HK33*P(6,14) + HK34*P(2,14) + HK35*P(1,14) - HK36*P(3,14) + HK37*P(0,14));
+Kfusion(15) = HK52*(HK28*P(15,22) - HK28*P(4,15) + HK32*P(15,23) - HK32*P(5,15) + HK33*P(6,15) + HK34*P(2,15) + HK35*P(1,15) - HK36*P(3,15) + HK37*P(0,15));
+Kfusion(16) = HK52*(HK28*P(16,22) - HK28*P(4,16) + HK32*P(16,23) - HK32*P(5,16) + HK33*P(6,16) + HK34*P(2,16) + HK35*P(1,16) - HK36*P(3,16) + HK37*P(0,16));
+Kfusion(17) = HK52*(HK28*P(17,22) - HK28*P(4,17) + HK32*P(17,23) - HK32*P(5,17) + HK33*P(6,17) + HK34*P(2,17) + HK35*P(1,17) - HK36*P(3,17) + HK37*P(0,17));
+Kfusion(18) = HK52*(HK28*P(18,22) - HK28*P(4,18) + HK32*P(18,23) - HK32*P(5,18) + HK33*P(6,18) + HK34*P(2,18) + HK35*P(1,18) - HK36*P(3,18) + HK37*P(0,18));
+Kfusion(19) = HK52*(HK28*P(19,22) - HK28*P(4,19) + HK32*P(19,23) - HK32*P(5,19) + HK33*P(6,19) + HK34*P(2,19) + HK35*P(1,19) - HK36*P(3,19) + HK37*P(0,19));
+Kfusion(20) = HK52*(HK28*P(20,22) - HK28*P(4,20) + HK32*P(20,23) - HK32*P(5,20) + HK33*P(6,20) + HK34*P(2,20) + HK35*P(1,20) - HK36*P(3,20) + HK37*P(0,20));
+Kfusion(21) = HK52*(HK28*P(21,22) - HK28*P(4,21) + HK32*P(21,23) - HK32*P(5,21) + HK33*P(6,21) + HK34*P(2,21) + HK35*P(1,21) - HK36*P(3,21) + HK37*P(0,21));
+Kfusion(22) = HK45*HK52;
+Kfusion(23) = HK42*HK52;
+Kfusion(24) = HK52*(HK28*P(22,24) - HK28*P(4,24) + HK32*P(23,24) - HK32*P(5,24) + HK33*P(6,24) + HK34*P(2,24) + HK35*P(1,24) - HK36*P(3,24) + HK37*P(0,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp

@@ -1,9 +1,9 @@
 // Equations for covariance matrix prediction, without process noise!
-const float PS0 = powf(q1, 2);
+const float PS0 = ecl::powf(q1, 2);
 const float PS1 = 0.25F*daxVar;
-const float PS2 = powf(q2, 2);
+const float PS2 = ecl::powf(q2, 2);
 const float PS3 = 0.25F*dayVar;
-const float PS4 = powf(q3, 2);
+const float PS4 = ecl::powf(q3, 2);
 const float PS5 = 0.25F*dazVar;
 const float PS6 = 0.5F*q1;
 const float PS7 = 0.5F*q2;
@@ -94,7 +94,7 @@ const float PS91 = PS59 + PS82;
 const float PS92 = PS69 + PS79;
 const float PS93 = PS49 - 2*PS51 + PS53;
 const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9;
-const float PS95 = powf(q0, 2);
+const float PS95 = ecl::powf(q0, 2);
 const float PS96 = -P(10,11)*PS34;
 const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96;
 const float PS98 = P(0,2)*PS13;
@@ -238,20 +238,20 @@ nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS
 nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122;
 nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147;
 nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167;
-nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar;
+nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*ecl::powf(PS56, 2) + PS185*ecl::powf(PS45, 2) + PS186 + ecl::powf(PS43, 2)*dvxVar;
 nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86;
 nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124;
 nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149;
 nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169;
 nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196;
-nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar;
+nextP(5,5) = PS185*ecl::powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*ecl::powf(PS76, 2) + PS213 + ecl::powf(PS75, 2)*dvyVar;
 nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94;
 nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125;
 nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150;
 nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170;
 nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200;
 nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218;
-nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar;
+nextP(6,6) = PS184*ecl::powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*ecl::powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + ecl::powf(PS87, 2)*dvzVar;
 nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt;
 nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt;
 nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt;
@@ -524,3 +524,28 @@ nextP(20,23) = P(20,23);
 nextP(21,23) = P(21,23);
 nextP(22,23) = P(22,23);
 nextP(23,23) = P(23,23);
+nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13;
+nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12;
+nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11;
+nextP(3,24) = P(0,24)*PS13 + P(1,24)*PS12 + P(10,24)*PS7 - P(11,24)*PS6 - P(12,24)*PS34 - P(2,24)*PS11 + P(3,24);
+nextP(4,24) = P(0,24)*PS174 + P(1,24)*PS173 + P(13,24)*PS43 + P(14,24)*PS172 - P(15,24)*PS171 + P(2,24)*PS175 - P(3,24)*PS176 + P(4,24);
+nextP(5,24) = -P(0,24)*PS202 - P(1,24)*PS204 - P(13,24)*PS193 + P(14,24)*PS75 + P(15,24)*PS190 + P(2,24)*PS201 + P(3,24)*PS203 + P(5,24);
+nextP(6,24) = P(0,24)*PS216 + P(1,24)*PS217 + P(13,24)*PS199 - P(14,24)*PS197 + P(15,24)*PS87 - P(2,24)*PS214 + P(3,24)*PS215 + P(6,24);
+nextP(7,24) = P(4,24)*dt + P(7,24);
+nextP(8,24) = P(5,24)*dt + P(8,24);
+nextP(9,24) = P(6,24)*dt + P(9,24);
+nextP(10,24) = P(10,24);
+nextP(11,24) = P(11,24);
+nextP(12,24) = P(12,24);
+nextP(13,24) = P(13,24);
+nextP(14,24) = P(14,24);
+nextP(15,24) = P(15,24);
+nextP(16,24) = P(16,24);
+nextP(17,24) = P(17,24);
+nextP(18,24) = P(18,24);
+nextP(19,24) = P(19,24);
+nextP(20,24) = P(20,24);
+nextP(21,24) = P(21,24);
+nextP(22,24) = P(22,24);
+nextP(23,24) = P(23,24);
+nextP(24,24) = P(24,24);

src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp

@@ -3,7 +3,7 @@
 #include <cstdlib>
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 inline float sq(float in) {
     return in * in;
@@ -73,7 +73,7 @@ int main()
         const float dvzVar = dvxVar;
 
         // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-        SquareMatrix24f P;
+        SquareMatrix25f P;
         for (int col=0; col<=23; col++) {
             for (int row=0; row<=col; row++) {
                 if (row == col) {
@@ -273,7 +273,7 @@ int main()
         const float PS185 = P(0,13)*PS60 + P(1,13)*PS74 + P(13,13)*PS166 - P(13,14)*PS165 - P(13,15)*PS70 - P(2,13)*PS72 + P(3,13)*PS62 + P(6,13);
         const float PS186 = P(0,6)*PS60 + P(1,6)*PS74 - P(2,6)*PS72 + P(3,6)*PS62 + P(6,13)*PS166 - P(6,14)*PS165 - P(6,15)*PS70 + P(6,6);
 
-        SquareMatrix24f nextP;
+        SquareMatrix25f nextP;
 
         nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
         nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
@@ -577,7 +577,7 @@ int main()
         nextP(23,23) = P(23,23);
 
         // save output and repeat calculation using legacy matlab generated code
-        SquareMatrix24f nextP_sympy;
+        SquareMatrix25f nextP_sympy;
         for (int col=0; col<=23; col++) {
             for (int row=0; row<=col; row++) {
                 nextP_sympy(row,col) = nextP(row,col);

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp

@@ -4,24 +4,24 @@
 #include "../../../../../matrix/matrix/math.hpp"
 #include "util.h"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
 
 int main()
 {
     // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
 
-	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
-    Vector24f Kfusion; // Optical flow observation Kalman gains
+	SparseVector25f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
+    Vector25f Kfusion; // Optical flow observation Kalman gains
 
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
+    Vector25f Hfusion_sympy;
+    Vector25f Kfusion_sympy;
 
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
+    Vector25f Hfusion_matlab;
+    Vector25f Kfusion_matlab;
 
 	const float R_LOS = sq(0.15f);
 
@@ -49,7 +49,7 @@ int main()
 	Tbs.identity();
 
     // create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp

@@ -1,65 +1,148 @@
 // X Axis Equations
 // Sub Expressions
-const float HK0 = Tbs(1,0)*q2;
-const float HK1 = Tbs(1,1)*q1;
-const float HK2 = Tbs(1,1)*q3;
-const float HK3 = Tbs(1,2)*q2;
-const float HK4 = Tbs(1,0)*q3;
-const float HK5 = Tbs(1,2)*q1;
-const float HK6 = -HK5;
-const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
-const float HK8 = 1.0F/range;
-const float HK9 = 2*HK8;
-const float HK10 = Tbs(1,1)*q2;
-const float HK11 = Tbs(1,2)*q3;
-const float HK12 = Tbs(1,1)*q0;
-const float HK13 = Tbs(1,2)*q0;
-const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11);
-const float HK15 = Tbs(1,0)*q1;
-const float HK16 = Tbs(1,0)*q0;
-const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13);
-const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6);
-const float HK19 = q0*q2;
-const float HK20 = q1*q3;
-const float HK21 = 2*Tbs(1,2);
-const float HK22 = q0*q3;
-const float HK23 = q1*q2;
-const float HK24 = 2*Tbs(1,1);
-const float HK25 = 2*powf(q2, 2);
-const float HK26 = 2*powf(q3, 2) - 1;
-const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26);
-const float HK28 = 2*Tbs(1,0);
-const float HK29 = q0*q1;
-const float HK30 = q2*q3;
-const float HK31 = 2*powf(q1, 2);
-const float HK32 = HK21*(HK29 - HK30) - HK28*(HK22 + HK23) + Tbs(1,1)*(HK26 + HK31);
-const float HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1);
-const float HK34 = 2*HK7;
-const float HK35 = 2*HK14;
-const float HK36 = 2*HK17;
-const float HK37 = 2*HK18;
-const float HK38 = -HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0) + HK35*P(0,1) + HK36*P(0,2) + HK37*P(0,3);
-const float HK39 = powf(range, -2);
-const float HK40 = -HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6) + HK35*P(1,6) + HK36*P(2,6) + HK37*P(3,6);
-const float HK41 = -HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5) + HK35*P(1,5) + HK36*P(2,5) + HK37*P(3,5);
-const float HK42 = -HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4) + HK35*P(1,4) + HK36*P(2,4) + HK37*P(3,4);
-const float HK43 = -HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3) + HK35*P(1,3) + HK36*P(2,3) + HK37*P(3,3);
-const float HK44 = -HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2) + HK35*P(1,2) + HK36*P(2,2) + HK37*P(2,3);
-const float HK45 = -HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1) + HK35*P(1,1) + HK36*P(1,2) + HK37*P(1,3);
-const float HK46 = HK8/(-HK27*HK39*HK42 - HK32*HK39*HK41 - HK33*HK39*HK40 - HK34*HK38*HK39 + HK35*HK39*HK45 + HK36*HK39*HK44 + HK37*HK39*HK43 + R_LOS);
+const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+const float HK3 = q0*q1;
+const float HK4 = q2*q3;
+const float HK5 = HK3 + HK4;
+const float HK6 = q0*q2;
+const float HK7 = q1*q3;
+const float HK8 = HK6 - HK7;
+const float HK9 = powf(q3, 2);
+const float HK10 = powf(q2, 2);
+const float HK11 = -HK10;
+const float HK12 = powf(q0, 2);
+const float HK13 = powf(q1, 2);
+const float HK14 = HK12 - HK13;
+const float HK15 = HK11 + HK14 + HK9;
+const float HK16 = HK15*Tbs(2,2) + 2*HK5*Tbs(2,1) - 2*HK8*Tbs(2,0);
+const float HK17 = 2*Tbs(1,1);
+const float HK18 = 2*Tbs(1,0);
+const float HK19 = HK15*Tbs(1,2) + HK17*HK5 - HK18*HK8;
+const float HK20 = q0*q3;
+const float HK21 = q1*q2;
+const float HK22 = 2*Tbs(1,2);
+const float HK23 = -HK9;
+const float HK24 = HK18*(HK20 + HK21) - HK22*(HK3 - HK4) + Tbs(1,1)*(HK10 + HK14 + HK23);
+const float HK25 = -HK17*(HK20 - HK21) + HK22*(HK6 + HK7) + Tbs(1,0)*(HK11 + HK12 + HK13 + HK23);
+const float HK26 = HK19*vd + HK24*ve + HK25*vn;
+const float HK27 = HK16*(HK0*vd + HK1*ve + HK2*vn) + HK26*(-Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0);
+const float HK28 = pd - ptd;
+const float HK29 = 1.0F/HK28;
+const float HK30 = 2*HK29;
+const float HK31 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+const float HK32 = HK16*(-HK0*ve + HK1*vd + HK31*vn) + HK26*(Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1);
+const float HK33 = -HK16*(HK0*vn - HK2*vd + HK31*ve) + HK26*(Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2);
+const float HK34 = HK16*(-HK1*vn + HK2*ve + HK31*vd) + HK26*(Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3);
+const float HK35 = HK16*HK29;
+const float HK36 = powf(HK28, -2);
+const float HK37 = HK16*HK26;
+const float HK38 = HK36*HK37;
+const float HK39 = HK16*HK25;
+const float HK40 = HK39*P(0,4);
+const float HK41 = HK16*HK24;
+const float HK42 = HK41*P(0,5);
+const float HK43 = HK16*HK19;
+const float HK44 = HK43*P(0,6);
+const float HK45 = HK26*HK35;
+const float HK46 = HK45*P(0,24);
+const float HK47 = HK45*P(0,9);
+const float HK48 = 2*HK34;
+const float HK49 = HK48*P(0,3);
+const float HK50 = 2*HK27;
+const float HK51 = HK50*P(0,0);
+const float HK52 = 2*HK32;
+const float HK53 = HK52*P(0,1);
+const float HK54 = 2*HK33;
+const float HK55 = HK54*P(0,2);
+const float HK56 = HK39*P(4,6);
+const float HK57 = HK41*P(5,6);
+const float HK58 = HK43*P(6,6);
+const float HK59 = HK45*P(6,9);
+const float HK60 = HK45*P(6,24);
+const float HK61 = HK48*P(3,6);
+const float HK62 = HK50*P(0,6);
+const float HK63 = HK52*P(1,6);
+const float HK64 = HK54*P(2,6);
+const float HK65 = HK39*P(4,5);
+const float HK66 = HK41*P(5,5);
+const float HK67 = HK43*P(5,6);
+const float HK68 = HK45*P(5,9);
+const float HK69 = HK45*P(5,24);
+const float HK70 = HK48*P(3,5);
+const float HK71 = HK50*P(0,5);
+const float HK72 = HK52*P(1,5);
+const float HK73 = HK54*P(2,5);
+const float HK74 = HK39*P(4,4);
+const float HK75 = HK41*P(4,5);
+const float HK76 = HK43*P(4,6);
+const float HK77 = HK45*P(4,9);
+const float HK78 = HK45*P(4,24);
+const float HK79 = HK48*P(3,4);
+const float HK80 = HK50*P(0,4);
+const float HK81 = HK52*P(1,4);
+const float HK82 = HK54*P(2,4);
+const float HK83 = HK39*P(4,24);
+const float HK84 = HK41*P(5,24);
+const float HK85 = HK43*P(6,24);
+const float HK86 = HK45*P(9,24);
+const float HK87 = HK45*P(24,24);
+const float HK88 = HK48*P(3,24);
+const float HK89 = HK50*P(0,24);
+const float HK90 = HK52*P(1,24);
+const float HK91 = HK54*P(2,24);
+const float HK92 = HK37/powf(HK28, 3);
+const float HK93 = HK39*P(4,9);
+const float HK94 = HK41*P(5,9);
+const float HK95 = HK43*P(6,9);
+const float HK96 = HK45*P(9,9);
+const float HK97 = -HK86;
+const float HK98 = HK48*P(3,9);
+const float HK99 = HK50*P(0,9);
+const float HK100 = HK52*P(1,9);
+const float HK101 = HK54*P(2,9);
+const float HK102 = HK39*P(3,4);
+const float HK103 = HK41*P(3,5);
+const float HK104 = HK43*P(3,6);
+const float HK105 = HK45*P(3,9);
+const float HK106 = HK45*P(3,24);
+const float HK107 = HK48*P(3,3);
+const float HK108 = HK50*P(0,3);
+const float HK109 = HK52*P(1,3);
+const float HK110 = HK54*P(2,3);
+const float HK111 = HK39*P(1,4);
+const float HK112 = HK41*P(1,5);
+const float HK113 = HK43*P(1,6);
+const float HK114 = HK45*P(1,9);
+const float HK115 = HK45*P(1,24);
+const float HK116 = HK48*P(1,3);
+const float HK117 = HK50*P(0,1);
+const float HK118 = HK52*P(1,1);
+const float HK119 = HK54*P(1,2);
+const float HK120 = HK39*P(2,4);
+const float HK121 = HK41*P(2,5);
+const float HK122 = HK43*P(2,6);
+const float HK123 = HK45*P(2,9);
+const float HK124 = HK45*P(2,24);
+const float HK125 = HK48*P(2,3);
+const float HK126 = HK50*P(0,2);
+const float HK127 = HK52*P(1,2);
+const float HK128 = HK54*P(2,2);
+const float HK129 = HK29/(HK36*HK39*(-HK74 - HK75 - HK76 + HK77 - HK78 - HK79 - HK80 - HK81 + HK82) + HK36*HK41*(-HK65 - HK66 - HK67 + HK68 - HK69 - HK70 - HK71 - HK72 + HK73) + HK36*HK43*(-HK56 - HK57 - HK58 + HK59 - HK60 - HK61 - HK62 - HK63 + HK64) + HK36*HK48*(-HK102 - HK103 - HK104 + HK105 - HK106 - HK107 - HK108 - HK109 + HK110) + HK36*HK50*(-HK40 - HK42 - HK44 - HK46 + HK47 - HK49 - HK51 - HK53 + HK55) + HK36*HK52*(-HK111 - HK112 - HK113 + HK114 - HK115 - HK116 - HK117 - HK118 + HK119) - HK36*HK54*(-HK120 - HK121 - HK122 + HK123 - HK124 - HK125 - HK126 - HK127 + HK128) - HK92*(-HK100 + HK101 - HK93 - HK94 - HK95 + HK96 + HK97 - HK98 - HK99) + HK92*(-HK83 - HK84 - HK85 + HK86 - HK87 - HK88 - HK89 - HK90 + HK91) - R_LOS);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = -HK7*HK9;
-Hfusion.at<1>() = HK14*HK9;
-Hfusion.at<2>() = HK17*HK9;
-Hfusion.at<3>() = HK18*HK9;
-Hfusion.at<4>() = -HK27*HK8;
-Hfusion.at<5>() = -HK32*HK8;
-Hfusion.at<6>() = -HK33*HK8;
+Hfusion.at<0>() = -HK27*HK30;
+Hfusion.at<1>() = -HK30*HK32;
+Hfusion.at<2>() = HK30*HK33;
+Hfusion.at<3>() = -HK30*HK34;
+Hfusion.at<4>() = -HK25*HK35;
+Hfusion.at<5>() = -HK24*HK35;
+Hfusion.at<6>() = -HK19*HK35;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
-Hfusion.at<9>() = 0;
+Hfusion.at<9>() = HK38;
 Hfusion.at<10>() = 0;
 Hfusion.at<11>() = 0;
 Hfusion.at<12>() = 0;
@@ -74,117 +157,118 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = -HK38;
 
 
 // Kalman gains
-Kfusion(0) = HK38*HK46;
-Kfusion(1) = HK45*HK46;
-Kfusion(2) = HK44*HK46;
-Kfusion(3) = HK43*HK46;
-Kfusion(4) = HK42*HK46;
-Kfusion(5) = HK41*HK46;
-Kfusion(6) = HK40*HK46;
-Kfusion(7) = HK46*(-HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7) + HK35*P(1,7) + HK36*P(2,7) + HK37*P(3,7));
-Kfusion(8) = HK46*(-HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8) + HK35*P(1,8) + HK36*P(2,8) + HK37*P(3,8));
-Kfusion(9) = HK46*(-HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9) + HK35*P(1,9) + HK36*P(2,9) + HK37*P(3,9));
-Kfusion(10) = HK46*(-HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10) + HK35*P(1,10) + HK36*P(2,10) + HK37*P(3,10));
-Kfusion(11) = HK46*(-HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11) + HK35*P(1,11) + HK36*P(2,11) + HK37*P(3,11));
-Kfusion(12) = HK46*(-HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12) + HK35*P(1,12) + HK36*P(2,12) + HK37*P(3,12));
-Kfusion(13) = HK46*(-HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13) + HK35*P(1,13) + HK36*P(2,13) + HK37*P(3,13));
-Kfusion(14) = HK46*(-HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14) + HK35*P(1,14) + HK36*P(2,14) + HK37*P(3,14));
-Kfusion(15) = HK46*(-HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15) + HK35*P(1,15) + HK36*P(2,15) + HK37*P(3,15));
-Kfusion(16) = HK46*(-HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16) + HK35*P(1,16) + HK36*P(2,16) + HK37*P(3,16));
-Kfusion(17) = HK46*(-HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17) + HK35*P(1,17) + HK36*P(2,17) + HK37*P(3,17));
-Kfusion(18) = HK46*(-HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18) + HK35*P(1,18) + HK36*P(2,18) + HK37*P(3,18));
-Kfusion(19) = HK46*(-HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19) + HK35*P(1,19) + HK36*P(2,19) + HK37*P(3,19));
-Kfusion(20) = HK46*(-HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20) + HK35*P(1,20) + HK36*P(2,20) + HK37*P(3,20));
-Kfusion(21) = HK46*(-HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21) + HK35*P(1,21) + HK36*P(2,21) + HK37*P(3,21));
-Kfusion(22) = HK46*(-HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22) + HK35*P(1,22) + HK36*P(2,22) + HK37*P(3,22));
-Kfusion(23) = HK46*(-HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23) + HK35*P(1,23) + HK36*P(2,23) + HK37*P(3,23));
+Kfusion(0) = HK129*(HK40 + HK42 + HK44 + HK46 - HK47 + HK49 + HK51 + HK53 - HK55);
+Kfusion(1) = HK129*(HK111 + HK112 + HK113 - HK114 + HK115 + HK116 + HK117 + HK118 - HK119);
+Kfusion(2) = HK129*(HK120 + HK121 + HK122 - HK123 + HK124 + HK125 + HK126 + HK127 - HK128);
+Kfusion(3) = HK129*(HK102 + HK103 + HK104 - HK105 + HK106 + HK107 + HK108 + HK109 - HK110);
+Kfusion(4) = HK129*(HK74 + HK75 + HK76 - HK77 + HK78 + HK79 + HK80 + HK81 - HK82);
+Kfusion(5) = HK129*(HK65 + HK66 + HK67 - HK68 + HK69 + HK70 + HK71 + HK72 - HK73);
+Kfusion(6) = HK129*(HK56 + HK57 + HK58 - HK59 + HK60 + HK61 + HK62 + HK63 - HK64);
+Kfusion(7) = HK129*(HK39*P(4,7) + HK41*P(5,7) + HK43*P(6,7) + HK45*P(7,24) - HK45*P(7,9) + HK48*P(3,7) + HK50*P(0,7) + HK52*P(1,7) - HK54*P(2,7));
+Kfusion(8) = HK129*(HK39*P(4,8) + HK41*P(5,8) + HK43*P(6,8) + HK45*P(8,24) - HK45*P(8,9) + HK48*P(3,8) + HK50*P(0,8) + HK52*P(1,8) - HK54*P(2,8));
+Kfusion(9) = HK129*(HK100 - HK101 + HK86 + HK93 + HK94 + HK95 - HK96 + HK98 + HK99);
+Kfusion(10) = HK129*(HK39*P(4,10) + HK41*P(5,10) + HK43*P(6,10) + HK45*P(10,24) - HK45*P(9,10) + HK48*P(3,10) + HK50*P(0,10) + HK52*P(1,10) - HK54*P(2,10));
+Kfusion(11) = HK129*(HK39*P(4,11) + HK41*P(5,11) + HK43*P(6,11) + HK45*P(11,24) - HK45*P(9,11) + HK48*P(3,11) + HK50*P(0,11) + HK52*P(1,11) - HK54*P(2,11));
+Kfusion(12) = HK129*(HK39*P(4,12) + HK41*P(5,12) + HK43*P(6,12) + HK45*P(12,24) - HK45*P(9,12) + HK48*P(3,12) + HK50*P(0,12) + HK52*P(1,12) - HK54*P(2,12));
+Kfusion(13) = HK129*(HK39*P(4,13) + HK41*P(5,13) + HK43*P(6,13) + HK45*P(13,24) - HK45*P(9,13) + HK48*P(3,13) + HK50*P(0,13) + HK52*P(1,13) - HK54*P(2,13));
+Kfusion(14) = HK129*(HK39*P(4,14) + HK41*P(5,14) + HK43*P(6,14) + HK45*P(14,24) - HK45*P(9,14) + HK48*P(3,14) + HK50*P(0,14) + HK52*P(1,14) - HK54*P(2,14));
+Kfusion(15) = HK129*(HK39*P(4,15) + HK41*P(5,15) + HK43*P(6,15) + HK45*P(15,24) - HK45*P(9,15) + HK48*P(3,15) + HK50*P(0,15) + HK52*P(1,15) - HK54*P(2,15));
+Kfusion(16) = HK129*(HK39*P(4,16) + HK41*P(5,16) + HK43*P(6,16) + HK45*P(16,24) - HK45*P(9,16) + HK48*P(3,16) + HK50*P(0,16) + HK52*P(1,16) - HK54*P(2,16));
+Kfusion(17) = HK129*(HK39*P(4,17) + HK41*P(5,17) + HK43*P(6,17) + HK45*P(17,24) - HK45*P(9,17) + HK48*P(3,17) + HK50*P(0,17) + HK52*P(1,17) - HK54*P(2,17));
+Kfusion(18) = HK129*(HK39*P(4,18) + HK41*P(5,18) + HK43*P(6,18) + HK45*P(18,24) - HK45*P(9,18) + HK48*P(3,18) + HK50*P(0,18) + HK52*P(1,18) - HK54*P(2,18));
+Kfusion(19) = HK129*(HK39*P(4,19) + HK41*P(5,19) + HK43*P(6,19) + HK45*P(19,24) - HK45*P(9,19) + HK48*P(3,19) + HK50*P(0,19) + HK52*P(1,19) - HK54*P(2,19));
+Kfusion(20) = HK129*(HK39*P(4,20) + HK41*P(5,20) + HK43*P(6,20) + HK45*P(20,24) - HK45*P(9,20) + HK48*P(3,20) + HK50*P(0,20) + HK52*P(1,20) - HK54*P(2,20));
+Kfusion(21) = HK129*(HK39*P(4,21) + HK41*P(5,21) + HK43*P(6,21) + HK45*P(21,24) - HK45*P(9,21) + HK48*P(3,21) + HK50*P(0,21) + HK52*P(1,21) - HK54*P(2,21));
+Kfusion(22) = HK129*(HK39*P(4,22) + HK41*P(5,22) + HK43*P(6,22) + HK45*P(22,24) - HK45*P(9,22) + HK48*P(3,22) + HK50*P(0,22) + HK52*P(1,22) - HK54*P(2,22));
+Kfusion(23) = HK129*(HK39*P(4,23) + HK41*P(5,23) + HK43*P(6,23) + HK45*P(23,24) - HK45*P(9,23) + HK48*P(3,23) + HK50*P(0,23) + HK52*P(1,23) - HK54*P(2,23));
+Kfusion(24) = HK129*(HK83 + HK84 + HK85 + HK87 + HK88 + HK89 + HK90 - HK91 + HK97);
 
 
 // Y Axis Equations
 // Sub Expressions
-const float HK0 = Tbs(0,0)*q3;
-const float HK1 = Tbs(0,2)*q1;
-const float HK2 = -HK1;
-const float HK3 = ve*(HK0 + HK2);
-const float HK4 = Tbs(0,0)*q2;
-const float HK5 = Tbs(0,1)*q1;
-const float HK6 = Tbs(0,1)*q3;
-const float HK7 = Tbs(0,2)*q2;
-const float HK8 = HK3 - vd*(HK4 - HK5) - vn*(HK6 - HK7);
-const float HK9 = 1.0F/range;
-const float HK10 = 2*HK9;
-const float HK11 = Tbs(0,2)*q0;
-const float HK12 = -HK4;
-const float HK13 = 2*HK5;
-const float HK14 = Tbs(0,1)*q0;
-const float HK15 = Tbs(0,1)*q2;
-const float HK16 = Tbs(0,2)*q3;
-const float HK17 = vd*(HK0 - 2*HK1 + HK14) + vn*(HK15 + HK16);
-const float HK18 = HK17 - ve*(HK11 + HK12 + HK13);
-const float HK19 = Tbs(0,0)*q0;
-const float HK20 = -HK6;
-const float HK21 = 2*HK7;
-const float HK22 = Tbs(0,0)*q1;
-const float HK23 = ve*(HK16 + HK22) + vn*(HK11 - 2*HK4 + HK5);
-const float HK24 = HK23 - vd*(HK19 + HK20 + HK21);
-const float HK25 = 2*HK0;
-const float HK26 = vd*(HK15 + HK22) + ve*(HK19 - 2*HK6 + HK7);
-const float HK27 = HK26 - vn*(HK14 + HK2 + HK25);
-const float HK28 = q0*q2;
-const float HK29 = q1*q3;
-const float HK30 = 2*Tbs(0,2);
-const float HK31 = HK30*(HK28 + HK29);
-const float HK32 = q0*q3;
-const float HK33 = q1*q2;
-const float HK34 = 2*Tbs(0,1);
-const float HK35 = 2*powf(q2, 2);
-const float HK36 = 2*powf(q3, 2);
-const float HK37 = HK36 - 1;
-const float HK38 = HK31 - HK34*(HK32 - HK33) - Tbs(0,0)*(HK35 + HK37);
-const float HK39 = 2*Tbs(0,0);
-const float HK40 = HK39*(HK32 + HK33);
-const float HK41 = q0*q1;
-const float HK42 = q2*q3;
-const float HK43 = 2*powf(q1, 2);
-const float HK44 = -HK30*(HK41 - HK42) + HK40 - Tbs(0,1)*(HK37 + HK43);
-const float HK45 = HK34*(HK41 + HK42);
-const float HK46 = -HK39*(HK28 - HK29) + HK45 - Tbs(0,2)*(HK35 + HK43 - 1);
-const float HK47 = 2*HK3 + 2*vd*(HK12 + HK5) + 2*vn*(HK20 + HK7);
-const float HK48 = -HK35;
-const float HK49 = 1 - HK36;
-const float HK50 = HK31 + HK34*(-HK32 + HK33) + Tbs(0,0)*(HK48 + HK49);
-const float HK51 = -HK43;
-const float HK52 = HK30*(-HK41 + HK42) + HK40 + Tbs(0,1)*(HK49 + HK51);
-const float HK53 = HK39*(-HK28 + HK29) + HK45 + Tbs(0,2)*(HK48 + HK51 + 1);
-const float HK54 = 2*HK17 + 2*ve*(-HK11 - HK13 + HK4);
-const float HK55 = 2*HK23 + 2*vd*(-HK19 - HK21 + HK6);
-const float HK56 = 2*HK26 + 2*vn*(HK1 - HK14 - HK25);
-const float HK57 = HK47*P(0,0) + HK50*P(0,4) + HK52*P(0,5) + HK53*P(0,6) + HK54*P(0,1) + HK55*P(0,2) + HK56*P(0,3);
-const float HK58 = powf(range, -2);
-const float HK59 = 2*HK58;
-const float HK60 = HK47*P(0,6) + HK50*P(4,6) + HK52*P(5,6) + HK53*P(6,6) + HK54*P(1,6) + HK55*P(2,6) + HK56*P(3,6);
-const float HK61 = HK47*P(0,5) + HK50*P(4,5) + HK52*P(5,5) + HK53*P(5,6) + HK54*P(1,5) + HK55*P(2,5) + HK56*P(3,5);
-const float HK62 = HK47*P(0,4) + HK50*P(4,4) + HK52*P(4,5) + HK53*P(4,6) + HK54*P(1,4) + HK55*P(2,4) + HK56*P(3,4);
-const float HK63 = HK47*P(0,3) + HK50*P(3,4) + HK52*P(3,5) + HK53*P(3,6) + HK54*P(1,3) + HK55*P(2,3) + HK56*P(3,3);
-const float HK64 = HK47*P(0,2) + HK50*P(2,4) + HK52*P(2,5) + HK53*P(2,6) + HK54*P(1,2) + HK55*P(2,2) + HK56*P(2,3);
-const float HK65 = HK47*P(0,1) + HK50*P(1,4) + HK52*P(1,5) + HK53*P(1,6) + HK54*P(1,1) + HK55*P(1,2) + HK56*P(1,3);
-const float HK66 = HK9/(HK18*HK59*HK65 + HK24*HK59*HK64 + HK27*HK59*HK63 + HK38*HK58*HK62 + HK44*HK58*HK61 + HK46*HK58*HK60 + HK57*HK59*HK8 + R_LOS);
+const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
+const float HK1 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
+const float HK2 = Tbs(0,0)*q0;
+const float HK3 = Tbs(0,2)*q2;
+const float HK4 = Tbs(0,1)*q3;
+const float HK5 = HK2 + HK3 - HK4;
+const float HK6 = q0*q1;
+const float HK7 = q2*q3;
+const float HK8 = HK6 + HK7;
+const float HK9 = q0*q2;
+const float HK10 = q1*q3;
+const float HK11 = -HK10 + HK9;
+const float HK12 = powf(q3, 2);
+const float HK13 = powf(q2, 2);
+const float HK14 = -HK13;
+const float HK15 = powf(q0, 2);
+const float HK16 = powf(q1, 2);
+const float HK17 = HK15 - HK16;
+const float HK18 = HK12 + HK14 + HK17;
+const float HK19 = -2*HK11*Tbs(2,0) + HK18*Tbs(2,2) + 2*HK8*Tbs(2,1);
+const float HK20 = 2*Tbs(0,0);
+const float HK21 = 2*Tbs(0,1);
+const float HK22 = HK18*Tbs(0,2) + HK21*HK8;
+const float HK23 = -HK11*HK20 + HK22;
+const float HK24 = 2*Tbs(0,2);
+const float HK25 = -HK12;
+const float HK26 = q0*q3;
+const float HK27 = q1*q2;
+const float HK28 = HK20*(HK26 + HK27) + Tbs(0,1)*(HK13 + HK17 + HK25);
+const float HK29 = -HK24*(HK6 - HK7) + HK28;
+const float HK30 = HK24*(HK10 + HK9) + Tbs(0,0)*(HK14 + HK15 + HK16 + HK25);
+const float HK31 = -HK21*(HK26 - HK27) + HK30;
+const float HK32 = HK23*vd + HK29*ve + HK31*vn;
+const float HK33 = HK19*(HK0*vd + HK1*ve + HK5*vn) + HK32*(-Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0);
+const float HK34 = pd - ptd;
+const float HK35 = 1.0F/HK34;
+const float HK36 = 2*HK35;
+const float HK37 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+const float HK38 = HK19*(-HK0*ve + HK1*vd + HK37*vn) + HK32*(Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1);
+const float HK39 = HK19*(HK0*vn + HK37*ve + vd*(-HK2 - HK3 + HK4));
+const float HK40 = (Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2)*(vd*(HK20*(HK10 - HK9) + HK22) + ve*(HK24*(-HK6 + HK7) + HK28) + vn*(HK21*(-HK26 + HK27) + HK30));
+const float HK41 = HK19*(-HK1*vn + HK37*vd + HK5*ve) + HK32*(Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3);
+const float HK42 = HK19*HK35;
+const float HK43 = powf(HK34, -2);
+const float HK44 = HK19*HK32;
+const float HK45 = HK43*HK44;
+const float HK46 = HK19*HK31;
+const float HK47 = HK19*HK29;
+const float HK48 = HK19*HK23;
+const float HK49 = HK32*HK42;
+const float HK50 = 2*HK41;
+const float HK51 = 2*HK33;
+const float HK52 = 2*HK38;
+const float HK53 = -2*HK39 + 2*HK40;
+const float HK54 = HK46*P(0,4) + HK47*P(0,5) + HK48*P(0,6) + HK49*P(0,24) - HK49*P(0,9) + HK50*P(0,3) + HK51*P(0,0) + HK52*P(0,1) - HK53*P(0,2);
+const float HK55 = HK46*P(4,6) + HK47*P(5,6) + HK48*P(6,6) + HK49*P(6,24) - HK49*P(6,9) + HK50*P(3,6) + HK51*P(0,6) + HK52*P(1,6) - HK53*P(2,6);
+const float HK56 = HK46*P(4,5) + HK47*P(5,5) + HK48*P(5,6) + HK49*P(5,24) - HK49*P(5,9) + HK50*P(3,5) + HK51*P(0,5) + HK52*P(1,5) - HK53*P(2,5);
+const float HK57 = HK46*P(4,4) + HK47*P(4,5) + HK48*P(4,6) + HK49*P(4,24) - HK49*P(4,9) + HK50*P(3,4) + HK51*P(0,4) + HK52*P(1,4) - HK53*P(2,4);
+const float HK58 = HK49*P(9,24);
+const float HK59 = HK46*P(4,24) + HK47*P(5,24) + HK48*P(6,24) + HK49*P(24,24) + HK50*P(3,24) + HK51*P(0,24) + HK52*P(1,24) - HK53*P(2,24) - HK58;
+const float HK60 = HK44/powf(HK34, 3);
+const float HK61 = HK46*P(4,9) + HK47*P(5,9) + HK48*P(6,9) - HK49*P(9,9) + HK50*P(3,9) + HK51*P(0,9) + HK52*P(1,9) - HK53*P(2,9) + HK58;
+const float HK62 = HK46*P(3,4) + HK47*P(3,5) + HK48*P(3,6) + HK49*P(3,24) - HK49*P(3,9) + HK50*P(3,3) + HK51*P(0,3) + HK52*P(1,3) - HK53*P(2,3);
+const float HK63 = HK46*P(1,4) + HK47*P(1,5) + HK48*P(1,6) + HK49*P(1,24) - HK49*P(1,9) + HK50*P(1,3) + HK51*P(0,1) + HK52*P(1,1) - HK53*P(1,2);
+const float HK64 = HK46*P(2,4) + HK47*P(2,5) + HK48*P(2,6) + HK49*P(2,24) - HK49*P(2,9) + HK50*P(2,3) + HK51*P(0,2) + HK52*P(1,2) - HK53*P(2,2);
+const float HK65 = HK35/(HK43*HK46*HK57 + HK43*HK47*HK56 + HK43*HK48*HK55 + HK43*HK50*HK62 + HK43*HK51*HK54 + HK43*HK52*HK63 - HK43*HK53*HK64 + HK59*HK60 - HK60*HK61 + R_LOS);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = -HK10*HK8;
-Hfusion.at<1>() = -HK10*HK18;
-Hfusion.at<2>() = -HK10*HK24;
-Hfusion.at<3>() = -HK10*HK27;
-Hfusion.at<4>() = -HK38*HK9;
-Hfusion.at<5>() = -HK44*HK9;
-Hfusion.at<6>() = -HK46*HK9;
+Hfusion.at<0>() = HK33*HK36;
+Hfusion.at<1>() = HK36*HK38;
+Hfusion.at<2>() = HK36*(HK39 - HK40);
+Hfusion.at<3>() = HK36*HK41;
+Hfusion.at<4>() = HK31*HK42;
+Hfusion.at<5>() = HK29*HK42;
+Hfusion.at<6>() = HK23*HK42;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
-Hfusion.at<9>() = 0;
+Hfusion.at<9>() = -HK45;
 Hfusion.at<10>() = 0;
 Hfusion.at<11>() = 0;
 Hfusion.at<12>() = 0;
@@ -199,30 +283,32 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = HK45;
 
 
 // Kalman gains
-Kfusion(0) = -HK57*HK66;
-Kfusion(1) = -HK65*HK66;
-Kfusion(2) = -HK64*HK66;
-Kfusion(3) = -HK63*HK66;
-Kfusion(4) = -HK62*HK66;
-Kfusion(5) = -HK61*HK66;
-Kfusion(6) = -HK60*HK66;
-Kfusion(7) = -HK66*(HK47*P(0,7) + HK50*P(4,7) + HK52*P(5,7) + HK53*P(6,7) + HK54*P(1,7) + HK55*P(2,7) + HK56*P(3,7));
-Kfusion(8) = -HK66*(HK47*P(0,8) + HK50*P(4,8) + HK52*P(5,8) + HK53*P(6,8) + HK54*P(1,8) + HK55*P(2,8) + HK56*P(3,8));
-Kfusion(9) = -HK66*(HK47*P(0,9) + HK50*P(4,9) + HK52*P(5,9) + HK53*P(6,9) + HK54*P(1,9) + HK55*P(2,9) + HK56*P(3,9));
-Kfusion(10) = -HK66*(HK47*P(0,10) + HK50*P(4,10) + HK52*P(5,10) + HK53*P(6,10) + HK54*P(1,10) + HK55*P(2,10) + HK56*P(3,10));
-Kfusion(11) = -HK66*(HK47*P(0,11) + HK50*P(4,11) + HK52*P(5,11) + HK53*P(6,11) + HK54*P(1,11) + HK55*P(2,11) + HK56*P(3,11));
-Kfusion(12) = -HK66*(HK47*P(0,12) + HK50*P(4,12) + HK52*P(5,12) + HK53*P(6,12) + HK54*P(1,12) + HK55*P(2,12) + HK56*P(3,12));
-Kfusion(13) = -HK66*(HK47*P(0,13) + HK50*P(4,13) + HK52*P(5,13) + HK53*P(6,13) + HK54*P(1,13) + HK55*P(2,13) + HK56*P(3,13));
-Kfusion(14) = -HK66*(HK47*P(0,14) + HK50*P(4,14) + HK52*P(5,14) + HK53*P(6,14) + HK54*P(1,14) + HK55*P(2,14) + HK56*P(3,14));
-Kfusion(15) = -HK66*(HK47*P(0,15) + HK50*P(4,15) + HK52*P(5,15) + HK53*P(6,15) + HK54*P(1,15) + HK55*P(2,15) + HK56*P(3,15));
-Kfusion(16) = -HK66*(HK47*P(0,16) + HK50*P(4,16) + HK52*P(5,16) + HK53*P(6,16) + HK54*P(1,16) + HK55*P(2,16) + HK56*P(3,16));
-Kfusion(17) = -HK66*(HK47*P(0,17) + HK50*P(4,17) + HK52*P(5,17) + HK53*P(6,17) + HK54*P(1,17) + HK55*P(2,17) + HK56*P(3,17));
-Kfusion(18) = -HK66*(HK47*P(0,18) + HK50*P(4,18) + HK52*P(5,18) + HK53*P(6,18) + HK54*P(1,18) + HK55*P(2,18) + HK56*P(3,18));
-Kfusion(19) = -HK66*(HK47*P(0,19) + HK50*P(4,19) + HK52*P(5,19) + HK53*P(6,19) + HK54*P(1,19) + HK55*P(2,19) + HK56*P(3,19));
-Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) + HK54*P(1,20) + HK55*P(2,20) + HK56*P(3,20));
-Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21));
-Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22));
-Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23));
+Kfusion(0) = HK54*HK65;
+Kfusion(1) = HK63*HK65;
+Kfusion(2) = HK64*HK65;
+Kfusion(3) = HK62*HK65;
+Kfusion(4) = HK57*HK65;
+Kfusion(5) = HK56*HK65;
+Kfusion(6) = HK55*HK65;
+Kfusion(7) = HK65*(HK46*P(4,7) + HK47*P(5,7) + HK48*P(6,7) + HK49*P(7,24) - HK49*P(7,9) + HK50*P(3,7) + HK51*P(0,7) + HK52*P(1,7) - HK53*P(2,7));
+Kfusion(8) = HK65*(HK46*P(4,8) + HK47*P(5,8) + HK48*P(6,8) + HK49*P(8,24) - HK49*P(8,9) + HK50*P(3,8) + HK51*P(0,8) + HK52*P(1,8) - HK53*P(2,8));
+Kfusion(9) = HK61*HK65;
+Kfusion(10) = HK65*(HK46*P(4,10) + HK47*P(5,10) + HK48*P(6,10) + HK49*P(10,24) - HK49*P(9,10) + HK50*P(3,10) + HK51*P(0,10) + HK52*P(1,10) - HK53*P(2,10));
+Kfusion(11) = HK65*(HK46*P(4,11) + HK47*P(5,11) + HK48*P(6,11) + HK49*P(11,24) - HK49*P(9,11) + HK50*P(3,11) + HK51*P(0,11) + HK52*P(1,11) - HK53*P(2,11));
+Kfusion(12) = HK65*(HK46*P(4,12) + HK47*P(5,12) + HK48*P(6,12) + HK49*P(12,24) - HK49*P(9,12) + HK50*P(3,12) + HK51*P(0,12) + HK52*P(1,12) - HK53*P(2,12));
+Kfusion(13) = HK65*(HK46*P(4,13) + HK47*P(5,13) + HK48*P(6,13) + HK49*P(13,24) - HK49*P(9,13) + HK50*P(3,13) + HK51*P(0,13) + HK52*P(1,13) - HK53*P(2,13));
+Kfusion(14) = HK65*(HK46*P(4,14) + HK47*P(5,14) + HK48*P(6,14) + HK49*P(14,24) - HK49*P(9,14) + HK50*P(3,14) + HK51*P(0,14) + HK52*P(1,14) - HK53*P(2,14));
+Kfusion(15) = HK65*(HK46*P(4,15) + HK47*P(5,15) + HK48*P(6,15) + HK49*P(15,24) - HK49*P(9,15) + HK50*P(3,15) + HK51*P(0,15) + HK52*P(1,15) - HK53*P(2,15));
+Kfusion(16) = HK65*(HK46*P(4,16) + HK47*P(5,16) + HK48*P(6,16) + HK49*P(16,24) - HK49*P(9,16) + HK50*P(3,16) + HK51*P(0,16) + HK52*P(1,16) - HK53*P(2,16));
+Kfusion(17) = HK65*(HK46*P(4,17) + HK47*P(5,17) + HK48*P(6,17) + HK49*P(17,24) - HK49*P(9,17) + HK50*P(3,17) + HK51*P(0,17) + HK52*P(1,17) - HK53*P(2,17));
+Kfusion(18) = HK65*(HK46*P(4,18) + HK47*P(5,18) + HK48*P(6,18) + HK49*P(18,24) - HK49*P(9,18) + HK50*P(3,18) + HK51*P(0,18) + HK52*P(1,18) - HK53*P(2,18));
+Kfusion(19) = HK65*(HK46*P(4,19) + HK47*P(5,19) + HK48*P(6,19) + HK49*P(19,24) - HK49*P(9,19) + HK50*P(3,19) + HK51*P(0,19) + HK52*P(1,19) - HK53*P(2,19));
+Kfusion(20) = HK65*(HK46*P(4,20) + HK47*P(5,20) + HK48*P(6,20) + HK49*P(20,24) - HK49*P(9,20) + HK50*P(3,20) + HK51*P(0,20) + HK52*P(1,20) - HK53*P(2,20));
+Kfusion(21) = HK65*(HK46*P(4,21) + HK47*P(5,21) + HK48*P(6,21) + HK49*P(21,24) - HK49*P(9,21) + HK50*P(3,21) + HK51*P(0,21) + HK52*P(1,21) - HK53*P(2,21));
+Kfusion(22) = HK65*(HK46*P(4,22) + HK47*P(5,22) + HK48*P(6,22) + HK49*P(22,24) - HK49*P(9,22) + HK50*P(3,22) + HK51*P(0,22) + HK52*P(1,22) - HK53*P(2,22));
+Kfusion(23) = HK65*(HK46*P(4,23) + HK47*P(5,23) + HK48*P(6,23) + HK49*P(23,24) - HK49*P(9,23) + HK50*P(3,23) + HK51*P(0,23) + HK52*P(1,23) - HK53*P(2,23));
+Kfusion(24) = HK59*HK65;

src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp

@@ -1,128 +1,203 @@
 // Sub Expressions
-const float HK0 = Tbs(1,0)*q2;
-const float HK1 = Tbs(1,1)*q1;
-const float HK2 = Tbs(1,1)*q3;
-const float HK3 = Tbs(1,2)*q2;
-const float HK4 = Tbs(1,0)*q3;
-const float HK5 = Tbs(1,2)*q1;
-const float HK6 = -HK5;
-const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
-const float HK8 = 1.0F/range;
-const float HK9 = 2*HK8;
-const float HK10 = Tbs(1,1)*q2;
-const float HK11 = Tbs(1,2)*q3;
-const float HK12 = Tbs(1,1)*q0;
-const float HK13 = Tbs(1,2)*q0;
-const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11);
-const float HK15 = Tbs(1,0)*q1;
-const float HK16 = Tbs(1,0)*q0;
-const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13);
-const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6);
-const float HK19 = q0*q2;
-const float HK20 = q1*q3;
-const float HK21 = HK19 + HK20;
-const float HK22 = 2*Tbs(1,2);
+const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
+const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
+const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
+const float HK3 = q0*q1;
+const float HK4 = q2*q3;
+const float HK5 = HK3 + HK4;
+const float HK6 = 2*HK5;
+const float HK7 = q0*q2;
+const float HK8 = q1*q3;
+const float HK9 = HK7 - HK8;
+const float HK10 = 2*HK9;
+const float HK11 = powf(q3, 2);
+const float HK12 = powf(q2, 2);
+const float HK13 = -HK12;
+const float HK14 = powf(q0, 2);
+const float HK15 = powf(q1, 2);
+const float HK16 = HK14 - HK15;
+const float HK17 = HK11 + HK13 + HK16;
+const float HK18 = -HK10*Tbs(2,0) + HK17*Tbs(2,2) + HK6*Tbs(2,1);
+const float HK19 = -Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0;
+const float HK20 = 2*Tbs(1,1);
+const float HK21 = 2*Tbs(1,0);
+const float HK22 = HK17*Tbs(1,2) + HK20*HK5 - HK21*HK9;
 const float HK23 = q0*q3;
 const float HK24 = q1*q2;
-const float HK25 = HK23 - HK24;
-const float HK26 = 2*Tbs(1,1);
-const float HK27 = 2*powf(q2, 2);
-const float HK28 = 2*powf(q3, 2);
-const float HK29 = HK28 - 1;
-const float HK30 = HK27 + HK29;
-const float HK31 = -HK21*HK22 + HK25*HK26 + HK30*Tbs(1,0);
-const float HK32 = HK23 + HK24;
-const float HK33 = 2*Tbs(1,0);
-const float HK34 = q0*q1;
-const float HK35 = q2*q3;
-const float HK36 = HK34 - HK35;
-const float HK37 = 2*powf(q1, 2);
-const float HK38 = HK29 + HK37;
-const float HK39 = HK22*HK36 - HK32*HK33 + HK38*Tbs(1,1);
-const float HK40 = HK34 + HK35;
-const float HK41 = HK19 - HK20;
-const float HK42 = HK27 + HK37 - 1;
-const float HK43 = -HK26*HK40 + HK33*HK41 + HK42*Tbs(1,2);
-const float HK44 = 2*HK7;
-const float HK45 = 2*HK14;
-const float HK46 = 2*HK17;
-const float HK47 = 2*HK18;
-const float HK48 = -HK31*P(0,4) - HK39*P(0,5) - HK43*P(0,6) - HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3);
-const float HK49 = powf(range, -2);
-const float HK50 = -HK31*P(4,6) - HK39*P(5,6) - HK43*P(6,6) - HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6);
-const float HK51 = -HK31*P(4,5) - HK39*P(5,5) - HK43*P(5,6) - HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5);
-const float HK52 = -HK31*P(4,4) - HK39*P(4,5) - HK43*P(4,6) - HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4);
-const float HK53 = -HK31*P(3,4) - HK39*P(3,5) - HK43*P(3,6) - HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3);
-const float HK54 = -HK31*P(2,4) - HK39*P(2,5) - HK43*P(2,6) - HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3);
-const float HK55 = -HK31*P(1,4) - HK39*P(1,5) - HK43*P(1,6) - HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3);
-const float HK56 = HK8/(-HK31*HK49*HK52 - HK39*HK49*HK51 - HK43*HK49*HK50 - HK44*HK48*HK49 + HK45*HK49*HK55 + HK46*HK49*HK54 + HK47*HK49*HK53 + R_LOS);
-const float HK57 = Tbs(0,0)*q3;
-const float HK58 = Tbs(0,2)*q1;
-const float HK59 = -HK58;
-const float HK60 = ve*(HK57 + HK59);
-const float HK61 = Tbs(0,0)*q2;
-const float HK62 = Tbs(0,1)*q1;
-const float HK63 = Tbs(0,1)*q3;
-const float HK64 = Tbs(0,2)*q2;
-const float HK65 = HK60 - vd*(HK61 - HK62) - vn*(HK63 - HK64);
-const float HK66 = Tbs(0,2)*q0;
-const float HK67 = -HK61;
-const float HK68 = 2*HK62;
-const float HK69 = Tbs(0,1)*q0;
-const float HK70 = Tbs(0,1)*q2;
-const float HK71 = Tbs(0,2)*q3;
-const float HK72 = vd*(HK57 - 2*HK58 + HK69) + vn*(HK70 + HK71);
-const float HK73 = HK72 - ve*(HK66 + HK67 + HK68);
-const float HK74 = Tbs(0,0)*q0;
-const float HK75 = -HK63;
-const float HK76 = 2*HK64;
-const float HK77 = Tbs(0,0)*q1;
-const float HK78 = ve*(HK71 + HK77) + vn*(-2*HK61 + HK62 + HK66);
-const float HK79 = HK78 - vd*(HK74 + HK75 + HK76);
-const float HK80 = 2*HK57;
-const float HK81 = vd*(HK70 + HK77) + ve*(-2*HK63 + HK64 + HK74);
-const float HK82 = HK81 - vn*(HK59 + HK69 + HK80);
-const float HK83 = 2*Tbs(0,2);
-const float HK84 = HK21*HK83;
-const float HK85 = 2*Tbs(0,1);
-const float HK86 = -HK25*HK85 - HK30*Tbs(0,0) + HK84;
-const float HK87 = 2*Tbs(0,0);
-const float HK88 = HK32*HK87;
-const float HK89 = -HK36*HK83 - HK38*Tbs(0,1) + HK88;
-const float HK90 = HK40*HK85;
-const float HK91 = -HK41*HK87 - HK42*Tbs(0,2) + HK90;
-const float HK92 = 2*HK60 + 2*vd*(HK62 + HK67) + 2*vn*(HK64 + HK75);
-const float HK93 = -HK27;
-const float HK94 = 1 - HK28;
-const float HK95 = HK84 + HK85*(-HK23 + HK24) + Tbs(0,0)*(HK93 + HK94);
-const float HK96 = -HK37;
-const float HK97 = HK83*(-HK34 + HK35) + HK88 + Tbs(0,1)*(HK94 + HK96);
-const float HK98 = HK87*(-HK19 + HK20) + HK90 + Tbs(0,2)*(HK93 + HK96 + 1);
-const float HK99 = 2*HK72 + 2*ve*(HK61 - HK66 - HK68);
-const float HK100 = 2*HK78 + 2*vd*(HK63 - HK74 - HK76);
-const float HK101 = 2*HK81 + 2*vn*(HK58 - HK69 - HK80);
-const float HK102 = HK100*P(0,2) + HK101*P(0,3) + HK92*P(0,0) + HK95*P(0,4) + HK97*P(0,5) + HK98*P(0,6) + HK99*P(0,1);
-const float HK103 = 2*HK49;
-const float HK104 = HK100*P(2,6) + HK101*P(3,6) + HK92*P(0,6) + HK95*P(4,6) + HK97*P(5,6) + HK98*P(6,6) + HK99*P(1,6);
-const float HK105 = HK100*P(2,5) + HK101*P(3,5) + HK92*P(0,5) + HK95*P(4,5) + HK97*P(5,5) + HK98*P(5,6) + HK99*P(1,5);
-const float HK106 = HK100*P(2,4) + HK101*P(3,4) + HK92*P(0,4) + HK95*P(4,4) + HK97*P(4,5) + HK98*P(4,6) + HK99*P(1,4);
-const float HK107 = HK100*P(2,3) + HK101*P(3,3) + HK92*P(0,3) + HK95*P(3,4) + HK97*P(3,5) + HK98*P(3,6) + HK99*P(1,3);
-const float HK108 = HK100*P(2,2) + HK101*P(2,3) + HK92*P(0,2) + HK95*P(2,4) + HK97*P(2,5) + HK98*P(2,6) + HK99*P(1,2);
-const float HK109 = HK100*P(1,2) + HK101*P(1,3) + HK92*P(0,1) + HK95*P(1,4) + HK97*P(1,5) + HK98*P(1,6) + HK99*P(1,1);
-const float HK110 = HK8/(HK102*HK103*HK65 + HK103*HK107*HK82 + HK103*HK108*HK79 + HK103*HK109*HK73 + HK104*HK49*HK91 + HK105*HK49*HK89 + HK106*HK49*HK86 + R_LOS);
+const float HK25 = HK23 + HK24;
+const float HK26 = HK3 - HK4;
+const float HK27 = 2*Tbs(1,2);
+const float HK28 = -HK11;
+const float HK29 = HK12 + HK16 + HK28;
+const float HK30 = HK21*HK25 - HK26*HK27 + HK29*Tbs(1,1);
+const float HK31 = HK7 + HK8;
+const float HK32 = HK23 - HK24;
+const float HK33 = HK13 + HK14 + HK15 + HK28;
+const float HK34 = -HK20*HK32 + HK27*HK31 + HK33*Tbs(1,0);
+const float HK35 = HK22*vd + HK30*ve + HK34*vn;
+const float HK36 = HK18*(HK0*vd + HK1*ve + HK2*vn) + HK19*HK35;
+const float HK37 = pd - ptd;
+const float HK38 = 1.0F/HK37;
+const float HK39 = 2*HK38;
+const float HK40 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
+const float HK41 = Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1;
+const float HK42 = HK18*(-HK0*ve + HK1*vd + HK40*vn) + HK35*HK41;
+const float HK43 = Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2;
+const float HK44 = -HK18*(HK0*vn - HK2*vd + HK40*ve) + HK35*HK43;
+const float HK45 = Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3;
+const float HK46 = HK18*(-HK1*vn + HK2*ve + HK40*vd) + HK35*HK45;
+const float HK47 = HK18*HK38;
+const float HK48 = powf(HK37, -2);
+const float HK49 = HK18*HK48;
+const float HK50 = HK35*HK49;
+const float HK51 = HK18*HK34;
+const float HK52 = HK51*P(0,4);
+const float HK53 = HK18*HK30;
+const float HK54 = HK53*P(0,5);
+const float HK55 = HK18*HK22;
+const float HK56 = HK55*P(0,6);
+const float HK57 = HK35*HK47;
+const float HK58 = HK57*P(0,24);
+const float HK59 = HK57*P(0,9);
+const float HK60 = 2*HK46;
+const float HK61 = HK60*P(0,3);
+const float HK62 = 2*HK36;
+const float HK63 = HK62*P(0,0);
+const float HK64 = 2*HK42;
+const float HK65 = HK64*P(0,1);
+const float HK66 = 2*HK44;
+const float HK67 = HK66*P(0,2);
+const float HK68 = HK51*P(4,6);
+const float HK69 = HK53*P(5,6);
+const float HK70 = HK55*P(6,6);
+const float HK71 = HK57*P(6,9);
+const float HK72 = HK57*P(6,24);
+const float HK73 = HK60*P(3,6);
+const float HK74 = HK62*P(0,6);
+const float HK75 = HK64*P(1,6);
+const float HK76 = HK66*P(2,6);
+const float HK77 = HK51*P(4,5);
+const float HK78 = HK53*P(5,5);
+const float HK79 = HK55*P(5,6);
+const float HK80 = HK57*P(5,9);
+const float HK81 = HK57*P(5,24);
+const float HK82 = HK60*P(3,5);
+const float HK83 = HK62*P(0,5);
+const float HK84 = HK64*P(1,5);
+const float HK85 = HK66*P(2,5);
+const float HK86 = HK51*P(4,4);
+const float HK87 = HK53*P(4,5);
+const float HK88 = HK55*P(4,6);
+const float HK89 = HK57*P(4,9);
+const float HK90 = HK57*P(4,24);
+const float HK91 = HK60*P(3,4);
+const float HK92 = HK62*P(0,4);
+const float HK93 = HK64*P(1,4);
+const float HK94 = HK66*P(2,4);
+const float HK95 = HK51*P(4,24);
+const float HK96 = HK53*P(5,24);
+const float HK97 = HK55*P(6,24);
+const float HK98 = HK57*P(9,24);
+const float HK99 = HK57*P(24,24);
+const float HK100 = HK60*P(3,24);
+const float HK101 = HK62*P(0,24);
+const float HK102 = HK64*P(1,24);
+const float HK103 = HK66*P(2,24);
+const float HK104 = HK18/powf(HK37, 3);
+const float HK105 = HK104*HK35;
+const float HK106 = HK51*P(4,9);
+const float HK107 = HK53*P(5,9);
+const float HK108 = HK55*P(6,9);
+const float HK109 = HK57*P(9,9);
+const float HK110 = -HK98;
+const float HK111 = HK60*P(3,9);
+const float HK112 = HK62*P(0,9);
+const float HK113 = HK64*P(1,9);
+const float HK114 = HK66*P(2,9);
+const float HK115 = HK51*P(3,4);
+const float HK116 = HK53*P(3,5);
+const float HK117 = HK55*P(3,6);
+const float HK118 = HK57*P(3,9);
+const float HK119 = HK57*P(3,24);
+const float HK120 = HK60*P(3,3);
+const float HK121 = HK62*P(0,3);
+const float HK122 = HK64*P(1,3);
+const float HK123 = HK66*P(2,3);
+const float HK124 = HK51*P(1,4);
+const float HK125 = HK53*P(1,5);
+const float HK126 = HK55*P(1,6);
+const float HK127 = HK57*P(1,9);
+const float HK128 = HK57*P(1,24);
+const float HK129 = HK60*P(1,3);
+const float HK130 = HK62*P(0,1);
+const float HK131 = HK64*P(1,1);
+const float HK132 = HK66*P(1,2);
+const float HK133 = HK51*P(2,4);
+const float HK134 = HK53*P(2,5);
+const float HK135 = HK55*P(2,6);
+const float HK136 = HK57*P(2,9);
+const float HK137 = HK57*P(2,24);
+const float HK138 = HK60*P(2,3);
+const float HK139 = HK62*P(0,2);
+const float HK140 = HK64*P(1,2);
+const float HK141 = HK66*P(2,2);
+const float HK142 = HK38/(HK105*(-HK100 - HK101 - HK102 + HK103 - HK95 - HK96 - HK97 + HK98 - HK99) - HK105*(-HK106 - HK107 - HK108 + HK109 + HK110 - HK111 - HK112 - HK113 + HK114) + HK22*HK49*(-HK68 - HK69 - HK70 + HK71 - HK72 - HK73 - HK74 - HK75 + HK76) + HK30*HK49*(-HK77 - HK78 - HK79 + HK80 - HK81 - HK82 - HK83 - HK84 + HK85) + HK34*HK49*(-HK86 - HK87 - HK88 + HK89 - HK90 - HK91 - HK92 - HK93 + HK94) + HK48*HK60*(-HK115 - HK116 - HK117 + HK118 - HK119 - HK120 - HK121 - HK122 + HK123) + HK48*HK62*(-HK52 - HK54 - HK56 - HK58 + HK59 - HK61 - HK63 - HK65 + HK67) + HK48*HK64*(-HK124 - HK125 - HK126 + HK127 - HK128 - HK129 - HK130 - HK131 + HK132) - HK48*HK66*(-HK133 - HK134 - HK135 + HK136 - HK137 - HK138 - HK139 - HK140 + HK141) - R_LOS);
+const float HK143 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
+const float HK144 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
+const float HK145 = Tbs(0,0)*q0;
+const float HK146 = Tbs(0,2)*q2;
+const float HK147 = Tbs(0,1)*q3;
+const float HK148 = HK145 + HK146 - HK147;
+const float HK149 = HK17*Tbs(0,2) + HK6*Tbs(0,1);
+const float HK150 = -HK10*Tbs(0,0) + HK149;
+const float HK151 = 2*Tbs(0,2);
+const float HK152 = 2*Tbs(0,0);
+const float HK153 = HK152*HK25 + HK29*Tbs(0,1);
+const float HK154 = -HK151*HK26 + HK153;
+const float HK155 = 2*Tbs(0,1);
+const float HK156 = HK151*HK31 + HK33*Tbs(0,0);
+const float HK157 = -HK155*HK32 + HK156;
+const float HK158 = HK150*vd + HK154*ve + HK157*vn;
+const float HK159 = HK158*HK19 + HK18*(HK143*vd + HK144*ve + HK148*vn);
+const float HK160 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
+const float HK161 = HK158*HK41 + HK18*(-HK143*ve + HK144*vd + HK160*vn);
+const float HK162 = HK18*(HK143*vn + HK160*ve + vd*(-HK145 - HK146 + HK147));
+const float HK163 = HK43*(vd*(HK149 + HK152*(-HK7 + HK8)) + ve*(HK151*(-HK3 + HK4) + HK153) + vn*(HK155*(-HK23 + HK24) + HK156));
+const float HK164 = HK158*HK45 + HK18*(-HK144*vn + HK148*ve + HK160*vd);
+const float HK165 = HK158*HK49;
+const float HK166 = HK157*HK18;
+const float HK167 = HK154*HK18;
+const float HK168 = HK150*HK18;
+const float HK169 = HK158*HK47;
+const float HK170 = 2*HK164;
+const float HK171 = 2*HK159;
+const float HK172 = 2*HK161;
+const float HK173 = -2*HK162 + 2*HK163;
+const float HK174 = HK166*P(0,4) + HK167*P(0,5) + HK168*P(0,6) + HK169*P(0,24) - HK169*P(0,9) + HK170*P(0,3) + HK171*P(0,0) + HK172*P(0,1) - HK173*P(0,2);
+const float HK175 = HK166*P(4,6) + HK167*P(5,6) + HK168*P(6,6) + HK169*P(6,24) - HK169*P(6,9) + HK170*P(3,6) + HK171*P(0,6) + HK172*P(1,6) - HK173*P(2,6);
+const float HK176 = HK166*P(4,5) + HK167*P(5,5) + HK168*P(5,6) + HK169*P(5,24) - HK169*P(5,9) + HK170*P(3,5) + HK171*P(0,5) + HK172*P(1,5) - HK173*P(2,5);
+const float HK177 = HK166*P(4,4) + HK167*P(4,5) + HK168*P(4,6) + HK169*P(4,24) - HK169*P(4,9) + HK170*P(3,4) + HK171*P(0,4) + HK172*P(1,4) - HK173*P(2,4);
+const float HK178 = HK169*P(9,24);
+const float HK179 = HK166*P(4,24) + HK167*P(5,24) + HK168*P(6,24) + HK169*P(24,24) + HK170*P(3,24) + HK171*P(0,24) + HK172*P(1,24) - HK173*P(2,24) - HK178;
+const float HK180 = HK104*HK158;
+const float HK181 = HK166*P(4,9) + HK167*P(5,9) + HK168*P(6,9) - HK169*P(9,9) + HK170*P(3,9) + HK171*P(0,9) + HK172*P(1,9) - HK173*P(2,9) + HK178;
+const float HK182 = HK166*P(3,4) + HK167*P(3,5) + HK168*P(3,6) + HK169*P(3,24) - HK169*P(3,9) + HK170*P(3,3) + HK171*P(0,3) + HK172*P(1,3) - HK173*P(2,3);
+const float HK183 = HK166*P(1,4) + HK167*P(1,5) + HK168*P(1,6) + HK169*P(1,24) - HK169*P(1,9) + HK170*P(1,3) + HK171*P(0,1) + HK172*P(1,1) - HK173*P(1,2);
+const float HK184 = HK166*P(2,4) + HK167*P(2,5) + HK168*P(2,6) + HK169*P(2,24) - HK169*P(2,9) + HK170*P(2,3) + HK171*P(0,2) + HK172*P(1,2) - HK173*P(2,2);
+const float HK185 = HK38/(HK150*HK175*HK49 + HK154*HK176*HK49 + HK157*HK177*HK49 + HK170*HK182*HK48 + HK171*HK174*HK48 + HK172*HK183*HK48 - HK173*HK184*HK48 + HK179*HK180 - HK180*HK181 + R_LOS);
 
 
 // Observation Jacobians - axis 0
-Hfusion.at<0>() = -HK7*HK9;
-Hfusion.at<1>() = HK14*HK9;
-Hfusion.at<2>() = HK17*HK9;
-Hfusion.at<3>() = HK18*HK9;
-Hfusion.at<4>() = -HK31*HK8;
-Hfusion.at<5>() = -HK39*HK8;
-Hfusion.at<6>() = -HK43*HK8;
+Hfusion.at<0>() = -HK36*HK39;
+Hfusion.at<1>() = -HK39*HK42;
+Hfusion.at<2>() = HK39*HK44;
+Hfusion.at<3>() = -HK39*HK46;
+Hfusion.at<4>() = -HK34*HK47;
+Hfusion.at<5>() = -HK30*HK47;
+Hfusion.at<6>() = -HK22*HK47;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
-Hfusion.at<9>() = 0;
+Hfusion.at<9>() = HK50;
 Hfusion.at<10>() = 0;
 Hfusion.at<11>() = 0;
 Hfusion.at<12>() = 0;
@@ -137,46 +212,48 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = -HK50;
 
 
 // Kalman gains - axis 0
-Kfusion(0) = HK48*HK56;
-Kfusion(1) = HK55*HK56;
-Kfusion(2) = HK54*HK56;
-Kfusion(3) = HK53*HK56;
-Kfusion(4) = HK52*HK56;
-Kfusion(5) = HK51*HK56;
-Kfusion(6) = HK50*HK56;
-Kfusion(7) = HK56*(-HK31*P(4,7) - HK39*P(5,7) - HK43*P(6,7) - HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7));
-Kfusion(8) = HK56*(-HK31*P(4,8) - HK39*P(5,8) - HK43*P(6,8) - HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8));
-Kfusion(9) = HK56*(-HK31*P(4,9) - HK39*P(5,9) - HK43*P(6,9) - HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9));
-Kfusion(10) = HK56*(-HK31*P(4,10) - HK39*P(5,10) - HK43*P(6,10) - HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10));
-Kfusion(11) = HK56*(-HK31*P(4,11) - HK39*P(5,11) - HK43*P(6,11) - HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11));
-Kfusion(12) = HK56*(-HK31*P(4,12) - HK39*P(5,12) - HK43*P(6,12) - HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12));
-Kfusion(13) = HK56*(-HK31*P(4,13) - HK39*P(5,13) - HK43*P(6,13) - HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13));
-Kfusion(14) = HK56*(-HK31*P(4,14) - HK39*P(5,14) - HK43*P(6,14) - HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14));
-Kfusion(15) = HK56*(-HK31*P(4,15) - HK39*P(5,15) - HK43*P(6,15) - HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15));
-Kfusion(16) = HK56*(-HK31*P(4,16) - HK39*P(5,16) - HK43*P(6,16) - HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16));
-Kfusion(17) = HK56*(-HK31*P(4,17) - HK39*P(5,17) - HK43*P(6,17) - HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17));
-Kfusion(18) = HK56*(-HK31*P(4,18) - HK39*P(5,18) - HK43*P(6,18) - HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18));
-Kfusion(19) = HK56*(-HK31*P(4,19) - HK39*P(5,19) - HK43*P(6,19) - HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19));
-Kfusion(20) = HK56*(-HK31*P(4,20) - HK39*P(5,20) - HK43*P(6,20) - HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20));
-Kfusion(21) = HK56*(-HK31*P(4,21) - HK39*P(5,21) - HK43*P(6,21) - HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21));
-Kfusion(22) = HK56*(-HK31*P(4,22) - HK39*P(5,22) - HK43*P(6,22) - HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22));
-Kfusion(23) = HK56*(-HK31*P(4,23) - HK39*P(5,23) - HK43*P(6,23) - HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23));
+Kfusion(0) = HK142*(HK52 + HK54 + HK56 + HK58 - HK59 + HK61 + HK63 + HK65 - HK67);
+Kfusion(1) = HK142*(HK124 + HK125 + HK126 - HK127 + HK128 + HK129 + HK130 + HK131 - HK132);
+Kfusion(2) = HK142*(HK133 + HK134 + HK135 - HK136 + HK137 + HK138 + HK139 + HK140 - HK141);
+Kfusion(3) = HK142*(HK115 + HK116 + HK117 - HK118 + HK119 + HK120 + HK121 + HK122 - HK123);
+Kfusion(4) = HK142*(HK86 + HK87 + HK88 - HK89 + HK90 + HK91 + HK92 + HK93 - HK94);
+Kfusion(5) = HK142*(HK77 + HK78 + HK79 - HK80 + HK81 + HK82 + HK83 + HK84 - HK85);
+Kfusion(6) = HK142*(HK68 + HK69 + HK70 - HK71 + HK72 + HK73 + HK74 + HK75 - HK76);
+Kfusion(7) = HK142*(HK51*P(4,7) + HK53*P(5,7) + HK55*P(6,7) + HK57*P(7,24) - HK57*P(7,9) + HK60*P(3,7) + HK62*P(0,7) + HK64*P(1,7) - HK66*P(2,7));
+Kfusion(8) = HK142*(HK51*P(4,8) + HK53*P(5,8) + HK55*P(6,8) + HK57*P(8,24) - HK57*P(8,9) + HK60*P(3,8) + HK62*P(0,8) + HK64*P(1,8) - HK66*P(2,8));
+Kfusion(9) = HK142*(HK106 + HK107 + HK108 - HK109 + HK111 + HK112 + HK113 - HK114 + HK98);
+Kfusion(10) = HK142*(HK51*P(4,10) + HK53*P(5,10) + HK55*P(6,10) + HK57*P(10,24) - HK57*P(9,10) + HK60*P(3,10) + HK62*P(0,10) + HK64*P(1,10) - HK66*P(2,10));
+Kfusion(11) = HK142*(HK51*P(4,11) + HK53*P(5,11) + HK55*P(6,11) + HK57*P(11,24) - HK57*P(9,11) + HK60*P(3,11) + HK62*P(0,11) + HK64*P(1,11) - HK66*P(2,11));
+Kfusion(12) = HK142*(HK51*P(4,12) + HK53*P(5,12) + HK55*P(6,12) + HK57*P(12,24) - HK57*P(9,12) + HK60*P(3,12) + HK62*P(0,12) + HK64*P(1,12) - HK66*P(2,12));
+Kfusion(13) = HK142*(HK51*P(4,13) + HK53*P(5,13) + HK55*P(6,13) + HK57*P(13,24) - HK57*P(9,13) + HK60*P(3,13) + HK62*P(0,13) + HK64*P(1,13) - HK66*P(2,13));
+Kfusion(14) = HK142*(HK51*P(4,14) + HK53*P(5,14) + HK55*P(6,14) + HK57*P(14,24) - HK57*P(9,14) + HK60*P(3,14) + HK62*P(0,14) + HK64*P(1,14) - HK66*P(2,14));
+Kfusion(15) = HK142*(HK51*P(4,15) + HK53*P(5,15) + HK55*P(6,15) + HK57*P(15,24) - HK57*P(9,15) + HK60*P(3,15) + HK62*P(0,15) + HK64*P(1,15) - HK66*P(2,15));
+Kfusion(16) = HK142*(HK51*P(4,16) + HK53*P(5,16) + HK55*P(6,16) + HK57*P(16,24) - HK57*P(9,16) + HK60*P(3,16) + HK62*P(0,16) + HK64*P(1,16) - HK66*P(2,16));
+Kfusion(17) = HK142*(HK51*P(4,17) + HK53*P(5,17) + HK55*P(6,17) + HK57*P(17,24) - HK57*P(9,17) + HK60*P(3,17) + HK62*P(0,17) + HK64*P(1,17) - HK66*P(2,17));
+Kfusion(18) = HK142*(HK51*P(4,18) + HK53*P(5,18) + HK55*P(6,18) + HK57*P(18,24) - HK57*P(9,18) + HK60*P(3,18) + HK62*P(0,18) + HK64*P(1,18) - HK66*P(2,18));
+Kfusion(19) = HK142*(HK51*P(4,19) + HK53*P(5,19) + HK55*P(6,19) + HK57*P(19,24) - HK57*P(9,19) + HK60*P(3,19) + HK62*P(0,19) + HK64*P(1,19) - HK66*P(2,19));
+Kfusion(20) = HK142*(HK51*P(4,20) + HK53*P(5,20) + HK55*P(6,20) + HK57*P(20,24) - HK57*P(9,20) + HK60*P(3,20) + HK62*P(0,20) + HK64*P(1,20) - HK66*P(2,20));
+Kfusion(21) = HK142*(HK51*P(4,21) + HK53*P(5,21) + HK55*P(6,21) + HK57*P(21,24) - HK57*P(9,21) + HK60*P(3,21) + HK62*P(0,21) + HK64*P(1,21) - HK66*P(2,21));
+Kfusion(22) = HK142*(HK51*P(4,22) + HK53*P(5,22) + HK55*P(6,22) + HK57*P(22,24) - HK57*P(9,22) + HK60*P(3,22) + HK62*P(0,22) + HK64*P(1,22) - HK66*P(2,22));
+Kfusion(23) = HK142*(HK51*P(4,23) + HK53*P(5,23) + HK55*P(6,23) + HK57*P(23,24) - HK57*P(9,23) + HK60*P(3,23) + HK62*P(0,23) + HK64*P(1,23) - HK66*P(2,23));
+Kfusion(24) = HK142*(HK100 + HK101 + HK102 - HK103 + HK110 + HK95 + HK96 + HK97 + HK99);
 
 
 // Observation Jacobians - axis 1
-Hfusion.at<0>() = -HK65*HK9;
-Hfusion.at<1>() = -HK73*HK9;
-Hfusion.at<2>() = -HK79*HK9;
-Hfusion.at<3>() = -HK82*HK9;
-Hfusion.at<4>() = -HK8*HK86;
-Hfusion.at<5>() = -HK8*HK89;
-Hfusion.at<6>() = -HK8*HK91;
+Hfusion.at<0>() = HK159*HK39;
+Hfusion.at<1>() = HK161*HK39;
+Hfusion.at<2>() = HK39*(HK162 - HK163);
+Hfusion.at<3>() = HK164*HK39;
+Hfusion.at<4>() = HK157*HK47;
+Hfusion.at<5>() = HK154*HK47;
+Hfusion.at<6>() = HK150*HK47;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
-Hfusion.at<9>() = 0;
+Hfusion.at<9>() = -HK165;
 Hfusion.at<10>() = 0;
 Hfusion.at<11>() = 0;
 Hfusion.at<12>() = 0;
@@ -191,30 +268,32 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = HK165;
 
 
 // Kalman gains - axis 1
-Kfusion(0) = -HK102*HK110;
-Kfusion(1) = -HK109*HK110;
-Kfusion(2) = -HK108*HK110;
-Kfusion(3) = -HK107*HK110;
-Kfusion(4) = -HK106*HK110;
-Kfusion(5) = -HK105*HK110;
-Kfusion(6) = -HK104*HK110;
-Kfusion(7) = -HK110*(HK100*P(2,7) + HK101*P(3,7) + HK92*P(0,7) + HK95*P(4,7) + HK97*P(5,7) + HK98*P(6,7) + HK99*P(1,7));
-Kfusion(8) = -HK110*(HK100*P(2,8) + HK101*P(3,8) + HK92*P(0,8) + HK95*P(4,8) + HK97*P(5,8) + HK98*P(6,8) + HK99*P(1,8));
-Kfusion(9) = -HK110*(HK100*P(2,9) + HK101*P(3,9) + HK92*P(0,9) + HK95*P(4,9) + HK97*P(5,9) + HK98*P(6,9) + HK99*P(1,9));
-Kfusion(10) = -HK110*(HK100*P(2,10) + HK101*P(3,10) + HK92*P(0,10) + HK95*P(4,10) + HK97*P(5,10) + HK98*P(6,10) + HK99*P(1,10));
-Kfusion(11) = -HK110*(HK100*P(2,11) + HK101*P(3,11) + HK92*P(0,11) + HK95*P(4,11) + HK97*P(5,11) + HK98*P(6,11) + HK99*P(1,11));
-Kfusion(12) = -HK110*(HK100*P(2,12) + HK101*P(3,12) + HK92*P(0,12) + HK95*P(4,12) + HK97*P(5,12) + HK98*P(6,12) + HK99*P(1,12));
-Kfusion(13) = -HK110*(HK100*P(2,13) + HK101*P(3,13) + HK92*P(0,13) + HK95*P(4,13) + HK97*P(5,13) + HK98*P(6,13) + HK99*P(1,13));
-Kfusion(14) = -HK110*(HK100*P(2,14) + HK101*P(3,14) + HK92*P(0,14) + HK95*P(4,14) + HK97*P(5,14) + HK98*P(6,14) + HK99*P(1,14));
-Kfusion(15) = -HK110*(HK100*P(2,15) + HK101*P(3,15) + HK92*P(0,15) + HK95*P(4,15) + HK97*P(5,15) + HK98*P(6,15) + HK99*P(1,15));
-Kfusion(16) = -HK110*(HK100*P(2,16) + HK101*P(3,16) + HK92*P(0,16) + HK95*P(4,16) + HK97*P(5,16) + HK98*P(6,16) + HK99*P(1,16));
-Kfusion(17) = -HK110*(HK100*P(2,17) + HK101*P(3,17) + HK92*P(0,17) + HK95*P(4,17) + HK97*P(5,17) + HK98*P(6,17) + HK99*P(1,17));
-Kfusion(18) = -HK110*(HK100*P(2,18) + HK101*P(3,18) + HK92*P(0,18) + HK95*P(4,18) + HK97*P(5,18) + HK98*P(6,18) + HK99*P(1,18));
-Kfusion(19) = -HK110*(HK100*P(2,19) + HK101*P(3,19) + HK92*P(0,19) + HK95*P(4,19) + HK97*P(5,19) + HK98*P(6,19) + HK99*P(1,19));
-Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20) + HK97*P(5,20) + HK98*P(6,20) + HK99*P(1,20));
-Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21));
-Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22));
-Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23));
+Kfusion(0) = HK174*HK185;
+Kfusion(1) = HK183*HK185;
+Kfusion(2) = HK184*HK185;
+Kfusion(3) = HK182*HK185;
+Kfusion(4) = HK177*HK185;
+Kfusion(5) = HK176*HK185;
+Kfusion(6) = HK175*HK185;
+Kfusion(7) = HK185*(HK166*P(4,7) + HK167*P(5,7) + HK168*P(6,7) + HK169*P(7,24) - HK169*P(7,9) + HK170*P(3,7) + HK171*P(0,7) + HK172*P(1,7) - HK173*P(2,7));
+Kfusion(8) = HK185*(HK166*P(4,8) + HK167*P(5,8) + HK168*P(6,8) + HK169*P(8,24) - HK169*P(8,9) + HK170*P(3,8) + HK171*P(0,8) + HK172*P(1,8) - HK173*P(2,8));
+Kfusion(9) = HK181*HK185;
+Kfusion(10) = HK185*(HK166*P(4,10) + HK167*P(5,10) + HK168*P(6,10) + HK169*P(10,24) - HK169*P(9,10) + HK170*P(3,10) + HK171*P(0,10) + HK172*P(1,10) - HK173*P(2,10));
+Kfusion(11) = HK185*(HK166*P(4,11) + HK167*P(5,11) + HK168*P(6,11) + HK169*P(11,24) - HK169*P(9,11) + HK170*P(3,11) + HK171*P(0,11) + HK172*P(1,11) - HK173*P(2,11));
+Kfusion(12) = HK185*(HK166*P(4,12) + HK167*P(5,12) + HK168*P(6,12) + HK169*P(12,24) - HK169*P(9,12) + HK170*P(3,12) + HK171*P(0,12) + HK172*P(1,12) - HK173*P(2,12));
+Kfusion(13) = HK185*(HK166*P(4,13) + HK167*P(5,13) + HK168*P(6,13) + HK169*P(13,24) - HK169*P(9,13) + HK170*P(3,13) + HK171*P(0,13) + HK172*P(1,13) - HK173*P(2,13));
+Kfusion(14) = HK185*(HK166*P(4,14) + HK167*P(5,14) + HK168*P(6,14) + HK169*P(14,24) - HK169*P(9,14) + HK170*P(3,14) + HK171*P(0,14) + HK172*P(1,14) - HK173*P(2,14));
+Kfusion(15) = HK185*(HK166*P(4,15) + HK167*P(5,15) + HK168*P(6,15) + HK169*P(15,24) - HK169*P(9,15) + HK170*P(3,15) + HK171*P(0,15) + HK172*P(1,15) - HK173*P(2,15));
+Kfusion(16) = HK185*(HK166*P(4,16) + HK167*P(5,16) + HK168*P(6,16) + HK169*P(16,24) - HK169*P(9,16) + HK170*P(3,16) + HK171*P(0,16) + HK172*P(1,16) - HK173*P(2,16));
+Kfusion(17) = HK185*(HK166*P(4,17) + HK167*P(5,17) + HK168*P(6,17) + HK169*P(17,24) - HK169*P(9,17) + HK170*P(3,17) + HK171*P(0,17) + HK172*P(1,17) - HK173*P(2,17));
+Kfusion(18) = HK185*(HK166*P(4,18) + HK167*P(5,18) + HK168*P(6,18) + HK169*P(18,24) - HK169*P(9,18) + HK170*P(3,18) + HK171*P(0,18) + HK172*P(1,18) - HK173*P(2,18));
+Kfusion(19) = HK185*(HK166*P(4,19) + HK167*P(5,19) + HK168*P(6,19) + HK169*P(19,24) - HK169*P(9,19) + HK170*P(3,19) + HK171*P(0,19) + HK172*P(1,19) - HK173*P(2,19));
+Kfusion(20) = HK185*(HK166*P(4,20) + HK167*P(5,20) + HK168*P(6,20) + HK169*P(20,24) - HK169*P(9,20) + HK170*P(3,20) + HK171*P(0,20) + HK172*P(1,20) - HK173*P(2,20));
+Kfusion(21) = HK185*(HK166*P(4,21) + HK167*P(5,21) + HK168*P(6,21) + HK169*P(21,24) - HK169*P(9,21) + HK170*P(3,21) + HK171*P(0,21) + HK172*P(1,21) - HK173*P(2,21));
+Kfusion(22) = HK185*(HK166*P(4,22) + HK167*P(5,22) + HK168*P(6,22) + HK169*P(22,24) - HK169*P(9,22) + HK170*P(3,22) + HK171*P(0,22) + HK172*P(1,22) - HK173*P(2,22));
+Kfusion(23) = HK185*(HK166*P(4,23) + HK167*P(5,23) + HK168*P(6,23) + HK169*P(23,24) - HK169*P(9,23) + HK170*P(3,23) + HK171*P(0,23) + HK172*P(1,23) - HK173*P(2,23));
+Kfusion(24) = HK179*HK185;

src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp

@@ -3,10 +3,10 @@
 #include <cstdlib>
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
 
 float sq(float in) {
 	return in * in;
@@ -17,7 +17,7 @@ int main()
 	// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
 
 	float H_YAW[24];
-	Vector24f Kfusion;
+	Vector25f Kfusion;
 	float _heading_innov_var;
 
 	const float R_YAW = sq(0.3f);
@@ -36,7 +36,7 @@ int main()
 	q3 /= length;
 
 	// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-	SquareMatrix24f P;
+	SquareMatrix25f P;
 	for (int col=0; col<=23; col++) {
 		for (int row=0; row<=col; row++) {
 			if (row == col) {
@@ -97,7 +97,7 @@ int main()
 
 	// calculate observation jacobian
 	// Observation jacobian and Kalman gain vectors
-	SparseVector24f<0,1,2,3> Hfusion;
+	SparseVector25f<0,1,2,3> Hfusion;
 	Hfusion.at<0>() = -HK16*HK18;
 	Hfusion.at<1>() = -HK18*HK24;
 	Hfusion.at<2>() = -HK18*HK25;
@@ -115,7 +115,7 @@ int main()
 
 	// save output and repeat calculation using legacy matlab generated code
 	float Hfusion_sympy[24] = {};
-	Vector24f Kfusion_sympy;
+	Vector25f Kfusion_sympy;
 	Hfusion_sympy[0] = Hfusion.at<0>();
 	Hfusion_sympy[1] = Hfusion.at<1>();
 	Hfusion_sympy[2] = Hfusion.at<2>();

src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp

@@ -1,38 +1,44 @@
 // Sub Expressions
-const float HK0 = cosf(ant_yaw);
-const float HK1 = sinf(ant_yaw);
-const float HK2 = q0*q3;
-const float HK3 = q1*q2;
-const float HK4 = 2*HK1;
-const float HK5 = 2*powf(q3, 2) - 1;
-const float HK6 = HK0*(HK5 + 2*powf(q2, 2)) + HK4*(HK2 - HK3);
-const float HK7 = 1.0F/HK6;
-const float HK8 = 2*HK0;
-const float HK9 = -HK1*(HK5 + 2*powf(q1, 2)) + HK8*(HK2 + HK3);
-const float HK10 = HK7*HK9;
-const float HK11 = HK1*HK10;
-const float HK12 = q3*(-HK0 + HK11);
-const float HK13 = powf(HK6, -2);
-const float HK14 = HK13*powf(HK9, 2) + 1;
-const float HK15 = 2*HK7/HK14;
-const float HK16 = HK0*q2;
-const float HK17 = HK1*q1;
-const float HK18 = HK11*q2 + HK16 - 2*HK17;
-const float HK19 = HK0*q1 + HK10*(-2*HK16 + HK17);
-const float HK20 = -HK0*q0 + HK10*(HK1*q0 + HK8*q3) + HK4*q3;
-const float HK21 = HK12*P(0,0) - HK18*P(0,1) - HK19*P(0,2) + HK20*P(0,3);
-const float HK22 = 4*HK13/powf(HK14, 2);
-const float HK23 = HK12*P(0,1) - HK18*P(1,1) - HK19*P(1,2) + HK20*P(1,3);
-const float HK24 = HK12*P(0,2) - HK18*P(1,2) - HK19*P(2,2) + HK20*P(2,3);
-const float HK25 = HK12*P(0,3) - HK18*P(1,3) - HK19*P(2,3) + HK20*P(3,3);
-const float HK26 = HK15/(HK12*HK21*HK22 - HK18*HK22*HK23 - HK19*HK22*HK24 + HK20*HK22*HK25 + R_YAW);
+const float HK0 = sinf(ant_yaw);
+const float HK1 = q0*q3;
+const float HK2 = q1*q2;
+const float HK3 = 2*HK0*(HK1 - HK2);
+const float HK4 = cosf(ant_yaw);
+const float HK5 = powf(q1, 2);
+const float HK6 = powf(q2, 2);
+const float HK7 = powf(q0, 2) - powf(q3, 2);
+const float HK8 = HK4*(HK5 - HK6 + HK7);
+const float HK9 = HK3 - HK8;
+const float HK10 = 1.0F/HK9;
+const float HK11 = HK4*q0;
+const float HK12 = HK0*q3;
+const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
+const float HK14 = HK10*HK13;
+const float HK15 = HK0*q0 + HK4*q3;
+const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
+const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
+const float HK18 = 2/HK17;
+const float HK19 = 1.0F/(-HK3 + HK8);
+const float HK20 = HK4*q1;
+const float HK21 = HK0*q2;
+const float HK22 = HK13*HK19;
+const float HK23 = HK0*q1 - HK4*q2;
+const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
+const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
+const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
+const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
+const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
+const float HK29 = 4/powf(HK17, 2);
+const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
+const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
+const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
 
 
 // Observation Jacobians
-Hfusion.at<0>() = HK12*HK15;
-Hfusion.at<1>() = -HK15*HK18;
-Hfusion.at<2>() = -HK15*HK19;
-Hfusion.at<3>() = HK15*HK20;
+Hfusion.at<0>() = -HK16*HK18;
+Hfusion.at<1>() = -HK18*HK24;
+Hfusion.at<2>() = -HK18*HK25;
+Hfusion.at<3>() = HK18*HK26;
 Hfusion.at<4>() = 0;
 Hfusion.at<5>() = 0;
 Hfusion.at<6>() = 0;
@@ -53,30 +59,32 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
-Kfusion(0) = HK21*HK26;
-Kfusion(1) = HK23*HK26;
-Kfusion(2) = HK24*HK26;
-Kfusion(3) = HK25*HK26;
-Kfusion(4) = HK26*(HK12*P(0,4) - HK18*P(1,4) - HK19*P(2,4) + HK20*P(3,4));
-Kfusion(5) = HK26*(HK12*P(0,5) - HK18*P(1,5) - HK19*P(2,5) + HK20*P(3,5));
-Kfusion(6) = HK26*(HK12*P(0,6) - HK18*P(1,6) - HK19*P(2,6) + HK20*P(3,6));
-Kfusion(7) = HK26*(HK12*P(0,7) - HK18*P(1,7) - HK19*P(2,7) + HK20*P(3,7));
-Kfusion(8) = HK26*(HK12*P(0,8) - HK18*P(1,8) - HK19*P(2,8) + HK20*P(3,8));
-Kfusion(9) = HK26*(HK12*P(0,9) - HK18*P(1,9) - HK19*P(2,9) + HK20*P(3,9));
-Kfusion(10) = HK26*(HK12*P(0,10) - HK18*P(1,10) - HK19*P(2,10) + HK20*P(3,10));
-Kfusion(11) = HK26*(HK12*P(0,11) - HK18*P(1,11) - HK19*P(2,11) + HK20*P(3,11));
-Kfusion(12) = HK26*(HK12*P(0,12) - HK18*P(1,12) - HK19*P(2,12) + HK20*P(3,12));
-Kfusion(13) = HK26*(HK12*P(0,13) - HK18*P(1,13) - HK19*P(2,13) + HK20*P(3,13));
-Kfusion(14) = HK26*(HK12*P(0,14) - HK18*P(1,14) - HK19*P(2,14) + HK20*P(3,14));
-Kfusion(15) = HK26*(HK12*P(0,15) - HK18*P(1,15) - HK19*P(2,15) + HK20*P(3,15));
-Kfusion(16) = HK26*(HK12*P(0,16) - HK18*P(1,16) - HK19*P(2,16) + HK20*P(3,16));
-Kfusion(17) = HK26*(HK12*P(0,17) - HK18*P(1,17) - HK19*P(2,17) + HK20*P(3,17));
-Kfusion(18) = HK26*(HK12*P(0,18) - HK18*P(1,18) - HK19*P(2,18) + HK20*P(3,18));
-Kfusion(19) = HK26*(HK12*P(0,19) - HK18*P(1,19) - HK19*P(2,19) + HK20*P(3,19));
-Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20));
-Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21));
-Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22));
-Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23));
+Kfusion(0) = HK27*HK32;
+Kfusion(1) = HK28*HK32;
+Kfusion(2) = HK30*HK32;
+Kfusion(3) = HK31*HK32;
+Kfusion(4) = HK32*(-HK16*P(0,4) - HK24*P(1,4) - HK25*P(2,4) + HK26*P(3,4));
+Kfusion(5) = HK32*(-HK16*P(0,5) - HK24*P(1,5) - HK25*P(2,5) + HK26*P(3,5));
+Kfusion(6) = HK32*(-HK16*P(0,6) - HK24*P(1,6) - HK25*P(2,6) + HK26*P(3,6));
+Kfusion(7) = HK32*(-HK16*P(0,7) - HK24*P(1,7) - HK25*P(2,7) + HK26*P(3,7));
+Kfusion(8) = HK32*(-HK16*P(0,8) - HK24*P(1,8) - HK25*P(2,8) + HK26*P(3,8));
+Kfusion(9) = HK32*(-HK16*P(0,9) - HK24*P(1,9) - HK25*P(2,9) + HK26*P(3,9));
+Kfusion(10) = HK32*(-HK16*P(0,10) - HK24*P(1,10) - HK25*P(2,10) + HK26*P(3,10));
+Kfusion(11) = HK32*(-HK16*P(0,11) - HK24*P(1,11) - HK25*P(2,11) + HK26*P(3,11));
+Kfusion(12) = HK32*(-HK16*P(0,12) - HK24*P(1,12) - HK25*P(2,12) + HK26*P(3,12));
+Kfusion(13) = HK32*(-HK16*P(0,13) - HK24*P(1,13) - HK25*P(2,13) + HK26*P(3,13));
+Kfusion(14) = HK32*(-HK16*P(0,14) - HK24*P(1,14) - HK25*P(2,14) + HK26*P(3,14));
+Kfusion(15) = HK32*(-HK16*P(0,15) - HK24*P(1,15) - HK25*P(2,15) + HK26*P(3,15));
+Kfusion(16) = HK32*(-HK16*P(0,16) - HK24*P(1,16) - HK25*P(2,16) + HK26*P(3,16));
+Kfusion(17) = HK32*(-HK16*P(0,17) - HK24*P(1,17) - HK25*P(2,17) + HK26*P(3,17));
+Kfusion(18) = HK32*(-HK16*P(0,18) - HK24*P(1,18) - HK25*P(2,18) + HK26*P(3,18));
+Kfusion(19) = HK32*(-HK16*P(0,19) - HK24*P(1,19) - HK25*P(2,19) + HK26*P(3,19));
+Kfusion(20) = HK32*(-HK16*P(0,20) - HK24*P(1,20) - HK25*P(2,20) + HK26*P(3,20));
+Kfusion(21) = HK32*(-HK16*P(0,21) - HK24*P(1,21) - HK25*P(2,21) + HK26*P(3,21));
+Kfusion(22) = HK32*(-HK16*P(0,22) - HK24*P(1,22) - HK25*P(2,22) + HK26*P(3,22));
+Kfusion(23) = HK32*(-HK16*P(0,23) - HK24*P(1,23) - HK25*P(2,23) + HK26*P(3,23));
+Kfusion(24) = HK32*(-HK16*P(0,24) - HK24*P(1,24) - HK25*P(2,24) + HK26*P(3,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/hagl_generated.cpp

@@ -0,0 +1,58 @@
+// Sub Expressions
+const float HK0 = 1.0F/(P(24,24) - 2*P(9,24) + P(9,9) + R_HAGL);
+
+
+// Observation Jacobians
+Hfusion.at<0>() = 0;
+Hfusion.at<1>() = 0;
+Hfusion.at<2>() = 0;
+Hfusion.at<3>() = 0;
+Hfusion.at<4>() = 0;
+Hfusion.at<5>() = 0;
+Hfusion.at<6>() = 0;
+Hfusion.at<7>() = 0;
+Hfusion.at<8>() = 0;
+Hfusion.at<9>() = -1;
+Hfusion.at<10>() = 0;
+Hfusion.at<11>() = 0;
+Hfusion.at<12>() = 0;
+Hfusion.at<13>() = 0;
+Hfusion.at<14>() = 0;
+Hfusion.at<15>() = 0;
+Hfusion.at<16>() = 0;
+Hfusion.at<17>() = 0;
+Hfusion.at<18>() = 0;
+Hfusion.at<19>() = 0;
+Hfusion.at<20>() = 0;
+Hfusion.at<21>() = 0;
+Hfusion.at<22>() = 0;
+Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 1;
+
+
+// Kalman gains
+Kfusion(0) = HK0*(P(0,24) - P(0,9));
+Kfusion(1) = HK0*(P(1,24) - P(1,9));
+Kfusion(2) = HK0*(P(2,24) - P(2,9));
+Kfusion(3) = HK0*(P(3,24) - P(3,9));
+Kfusion(4) = HK0*(P(4,24) - P(4,9));
+Kfusion(5) = HK0*(P(5,24) - P(5,9));
+Kfusion(6) = HK0*(P(6,24) - P(6,9));
+Kfusion(7) = HK0*(P(7,24) - P(7,9));
+Kfusion(8) = HK0*(P(8,24) - P(8,9));
+Kfusion(9) = HK0*(P(9,24) - P(9,9));
+Kfusion(10) = HK0*(P(10,24) - P(9,10));
+Kfusion(11) = HK0*(P(11,24) - P(9,11));
+Kfusion(12) = HK0*(P(12,24) - P(9,12));
+Kfusion(13) = HK0*(P(13,24) - P(9,13));
+Kfusion(14) = HK0*(P(14,24) - P(9,14));
+Kfusion(15) = HK0*(P(15,24) - P(9,15));
+Kfusion(16) = HK0*(P(16,24) - P(9,16));
+Kfusion(17) = HK0*(P(17,24) - P(9,17));
+Kfusion(18) = HK0*(P(18,24) - P(9,18));
+Kfusion(19) = HK0*(P(19,24) - P(9,19));
+Kfusion(20) = HK0*(P(20,24) - P(9,20));
+Kfusion(21) = HK0*(P(21,24) - P(9,21));
+Kfusion(22) = HK0*(P(22,24) - P(9,22));
+Kfusion(23) = HK0*(P(23,24) - P(9,23));
+Kfusion(24) = HK0*(P(24,24) - P(9,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp

@@ -3,8 +3,8 @@
 #include <cstdlib>
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 float sq(float in) {
     return in * in;
@@ -15,8 +15,8 @@ int main()
     // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
 
 	float Hfusion[24] = {};
-    Vector24f H_DECL;
-    Vector24f Kfusion;
+    Vector25f H_DECL;
+    Vector25f Kfusion;
     float decl_innov_var;
 
 	const float R_DECL = sq(0.3f);
@@ -40,7 +40,7 @@ int main()
     const float h_field_min = 1e-3f;
 
     // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {
@@ -95,7 +95,7 @@ int main()
 
     // save output and repeat calculation using legacy matlab generated code
     float Hfusion_sympy[24];
-    Vector24f Kfusion_sympy;
+    Vector25f Kfusion_sympy;
     for (int row=0; row<24; row++) {
         Hfusion_sympy[row] = Hfusion[row];
         Kfusion_sympy(row) = Kfusion(row);

src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp

@@ -36,6 +36,7 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
@@ -63,3 +64,4 @@ Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
 Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
 Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
 Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
+Kfusion(24) = -HK9*(HK5*P(16,24) - P(17,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp

@@ -4,10 +4,10 @@
 #include "../../../../../matrix/matrix/math.hpp"
 #include "util.h"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 template<int ... Idxs>
-using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
 
 int main()
 {
@@ -15,13 +15,13 @@ int main()
 
     float airspeed_innov_var;
 
-    Vector24f Kfusion; // Kalman gain vector
+    Vector25f Kfusion; // Kalman gain vector
 
-    Vector24f Hfusion_sympy;
-    Vector24f Kfusion_sympy;
+    Vector25f Hfusion_sympy;
+    Vector25f Kfusion_sympy;
 
-    Vector24f Hfusion_matlab;
-    Vector24f Kfusion_matlab;
+    Vector25f Hfusion_matlab;
+    Vector25f Kfusion_matlab;
 
     const float R_TAS = sq(1.5f);
 
@@ -37,7 +37,7 @@ int main()
     const float vwe = 3.0f;
 
     // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
-    SquareMatrix24f P;
+    SquareMatrix25f P;
     for (int col=0; col<=23; col++) {
         for (int row=0; row<=col; row++) {
             if (row == col) {
@@ -77,7 +77,7 @@ int main()
         const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
 
         // Observation Jacobians
-        SparseVector24f<4,5,6,22,23> Hfusion;
+        SparseVector25f<4,5,6,22,23> Hfusion;
         Hfusion.at<4>() = HK4;
         Hfusion.at<5>() = HK5;
         Hfusion.at<6>() = HK3*vd;
@@ -121,7 +121,7 @@ int main()
         SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
 
         // Observation Jacobian
-        Vector24f H_TAS = {};
+        Vector25f H_TAS = {};
         H_TAS(4) = SH_TAS[2];
         H_TAS(5) = SH_TAS[1];
         H_TAS(6) = vd*SH_TAS[0];

src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp

@@ -43,6 +43,7 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = -HK4;
 Hfusion.at<23>() = -HK5;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains
@@ -70,3 +71,4 @@ Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P
 Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
 Kfusion(22) = HK15*HK16;
 Kfusion(23) = HK14*HK16;
+Kfusion(24) = HK16*(-HK0*P(22,24) + HK0*P(4,24) - HK1*P(23,24) + HK1*P(5,24) + P(6,24)*vd);

src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h

@@ -1,6 +1,6 @@
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 inline float sq(float in) {
     return in * in;

src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp

@@ -1,123 +1,37 @@
 // axis 0
-const float HK0 = q3*ve;
-const float HK1 = q2*vd;
-const float HK2 = q2*ve + q3*vd;
-const float HK3 = q1*ve;
-const float HK4 = q0*vd;
-const float HK5 = 2*vn;
-const float HK6 = HK5*q2;
-const float HK7 = -HK5*q3 + q0*ve + q1*vd;
-const float HK8 = 2*powf(q2, 2);
-const float HK9 = 2*powf(q3, 2);
-const float HK10 = q0*q3 + q1*q2;
-const float HK11 = q1*q3;
-const float HK12 = q0*q2;
-const float HK13 = 2*HK2;
-const float HK14 = 2*HK10;
-const float HK15 = -2*HK0 + 2*HK1;
-const float HK16 = -2*HK11 + 2*HK12;
-const float HK17 = 2*HK7;
-const float HK18 = HK8 + HK9 - 1;
-const float HK19 = -2*HK3 + 2*HK4 + 2*HK6;
-const float HK20 = -HK13*P(0,1) - HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,6) - HK17*P(0,3) + HK18*P(0,4) + HK19*P(0,2);
-const float HK21 = -HK13*P(1,5) - HK14*P(5,5) + HK15*P(0,5) + HK16*P(5,6) - HK17*P(3,5) + HK18*P(4,5) + HK19*P(2,5);
-const float HK22 = -HK13*P(1,1) - HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,6) - HK17*P(1,3) + HK18*P(1,4) + HK19*P(1,2);
-const float HK23 = -HK13*P(1,6) - HK14*P(5,6) + HK15*P(0,6) + HK16*P(6,6) - HK17*P(3,6) + HK18*P(4,6) + HK19*P(2,6);
-const float HK24 = -HK13*P(1,4) - HK14*P(4,5) + HK15*P(0,4) + HK16*P(4,6) - HK17*P(3,4) + HK18*P(4,4) + HK19*P(2,4);
-const float HK25 = -HK13*P(1,3) - HK14*P(3,5) + HK15*P(0,3) + HK16*P(3,6) - HK17*P(3,3) + HK18*P(3,4) + HK19*P(2,3);
-const float HK26 = -HK13*P(1,2) - HK14*P(2,5) + HK15*P(0,2) + HK16*P(2,6) - HK17*P(2,3) + HK18*P(2,4) + HK19*P(2,2);
-const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL);
-
-
-H_VEL(0) = 2*HK0 - 2*HK1;
-H_VEL(1) = 2*HK2;
-H_VEL(2) = 2*HK3 - 2*HK4 - 2*HK6;
-H_VEL(3) = 2*HK7;
-H_VEL(4) = -HK8 - HK9 + 1;
-H_VEL(5) = 2*HK10;
-H_VEL(6) = 2*HK11 - 2*HK12;
-H_VEL(7) = 0;
-H_VEL(8) = 0;
-H_VEL(9) = 0;
-H_VEL(10) = 0;
-H_VEL(11) = 0;
-H_VEL(12) = 0;
-H_VEL(13) = 0;
-H_VEL(14) = 0;
-H_VEL(15) = 0;
-H_VEL(16) = 0;
-H_VEL(17) = 0;
-H_VEL(18) = 0;
-H_VEL(19) = 0;
-H_VEL(20) = 0;
-H_VEL(21) = 0;
-H_VEL(22) = 0;
-H_VEL(23) = 0;
-
-
-Kfusion(0) = -HK20*HK27;
-Kfusion(1) = -HK22*HK27;
-Kfusion(2) = -HK26*HK27;
-Kfusion(3) = -HK25*HK27;
-Kfusion(4) = -HK24*HK27;
-Kfusion(5) = -HK21*HK27;
-Kfusion(6) = -HK23*HK27;
-Kfusion(7) = -HK27*(-HK13*P(1,7) - HK14*P(5,7) + HK15*P(0,7) + HK16*P(6,7) - HK17*P(3,7) + HK18*P(4,7) + HK19*P(2,7));
-Kfusion(8) = -HK27*(-HK13*P(1,8) - HK14*P(5,8) + HK15*P(0,8) + HK16*P(6,8) - HK17*P(3,8) + HK18*P(4,8) + HK19*P(2,8));
-Kfusion(9) = -HK27*(-HK13*P(1,9) - HK14*P(5,9) + HK15*P(0,9) + HK16*P(6,9) - HK17*P(3,9) + HK18*P(4,9) + HK19*P(2,9));
-Kfusion(10) = -HK27*(-HK13*P(1,10) - HK14*P(5,10) + HK15*P(0,10) + HK16*P(6,10) - HK17*P(3,10) + HK18*P(4,10) + HK19*P(2,10));
-Kfusion(11) = -HK27*(-HK13*P(1,11) - HK14*P(5,11) + HK15*P(0,11) + HK16*P(6,11) - HK17*P(3,11) + HK18*P(4,11) + HK19*P(2,11));
-Kfusion(12) = -HK27*(-HK13*P(1,12) - HK14*P(5,12) + HK15*P(0,12) + HK16*P(6,12) - HK17*P(3,12) + HK18*P(4,12) + HK19*P(2,12));
-Kfusion(13) = -HK27*(-HK13*P(1,13) - HK14*P(5,13) + HK15*P(0,13) + HK16*P(6,13) - HK17*P(3,13) + HK18*P(4,13) + HK19*P(2,13));
-Kfusion(14) = -HK27*(-HK13*P(1,14) - HK14*P(5,14) + HK15*P(0,14) + HK16*P(6,14) - HK17*P(3,14) + HK18*P(4,14) + HK19*P(2,14));
-Kfusion(15) = -HK27*(-HK13*P(1,15) - HK14*P(5,15) + HK15*P(0,15) + HK16*P(6,15) - HK17*P(3,15) + HK18*P(4,15) + HK19*P(2,15));
-Kfusion(16) = -HK27*(-HK13*P(1,16) - HK14*P(5,16) + HK15*P(0,16) + HK16*P(6,16) - HK17*P(3,16) + HK18*P(4,16) + HK19*P(2,16));
-Kfusion(17) = -HK27*(-HK13*P(1,17) - HK14*P(5,17) + HK15*P(0,17) + HK16*P(6,17) - HK17*P(3,17) + HK18*P(4,17) + HK19*P(2,17));
-Kfusion(18) = -HK27*(-HK13*P(1,18) - HK14*P(5,18) + HK15*P(0,18) + HK16*P(6,18) - HK17*P(3,18) + HK18*P(4,18) + HK19*P(2,18));
-Kfusion(19) = -HK27*(-HK13*P(1,19) - HK14*P(5,19) + HK15*P(0,19) + HK16*P(6,19) - HK17*P(3,19) + HK18*P(4,19) + HK19*P(2,19));
-Kfusion(20) = -HK27*(-HK13*P(1,20) - HK14*P(5,20) + HK15*P(0,20) + HK16*P(6,20) - HK17*P(3,20) + HK18*P(4,20) + HK19*P(2,20));
-Kfusion(21) = -HK27*(-HK13*P(1,21) - HK14*P(5,21) + HK15*P(0,21) + HK16*P(6,21) - HK17*P(3,21) + HK18*P(4,21) + HK19*P(2,21));
-Kfusion(22) = -HK27*(-HK13*P(1,22) - HK14*P(5,22) + HK15*P(0,22) + HK16*P(6,22) - HK17*P(3,22) + HK18*P(4,22) + HK19*P(2,22));
-Kfusion(23) = -HK27*(-HK13*P(1,23) - HK14*P(5,23) + HK15*P(0,23) + HK16*P(6,23) - HK17*P(3,23) + HK18*P(4,23) + HK19*P(2,23));
-
-
-// axis 1
-const float HK0 = q1*vd - q3*vn;
-const float HK1 = 2*ve;
-const float HK2 = -HK1*q1 + q0*vd + q2*vn;
-const float HK3 = q1*vn + q3*vd;
-const float HK4 = q2*vd;
-const float HK5 = q0*vn;
-const float HK6 = HK1*q3;
-const float HK7 = q1*q2;
-const float HK8 = q0*q3;
-const float HK9 = 2*powf(q1, 2);
-const float HK10 = 2*powf(q3, 2);
-const float HK11 = q0*q1 + q2*q3;
-const float HK12 = 2*HK3;
-const float HK13 = 2*HK11;
-const float HK14 = 2*HK0;
-const float HK15 = -2*HK7 + 2*HK8;
-const float HK16 = 2*HK2;
-const float HK17 = -2*HK4 + 2*HK5 + 2*HK6;
-const float HK18 = HK10 + HK9 - 1;
-const float HK19 = HK12*P(0,2) + HK13*P(0,6) + HK14*P(0,0) - HK15*P(0,4) + HK16*P(0,1) - HK17*P(0,3) - HK18*P(0,5);
-const float HK20 = HK12*P(2,6) + HK13*P(6,6) + HK14*P(0,6) - HK15*P(4,6) + HK16*P(1,6) - HK17*P(3,6) - HK18*P(5,6);
-const float HK21 = HK12*P(2,2) + HK13*P(2,6) + HK14*P(0,2) - HK15*P(2,4) + HK16*P(1,2) - HK17*P(2,3) - HK18*P(2,5);
-const float HK22 = HK12*P(2,4) + HK13*P(4,6) + HK14*P(0,4) - HK15*P(4,4) + HK16*P(1,4) - HK17*P(3,4) - HK18*P(4,5);
-const float HK23 = HK12*P(1,2) + HK13*P(1,6) + HK14*P(0,1) - HK15*P(1,4) + HK16*P(1,1) - HK17*P(1,3) - HK18*P(1,5);
-const float HK24 = HK12*P(2,5) + HK13*P(5,6) + HK14*P(0,5) - HK15*P(4,5) + HK16*P(1,5) - HK17*P(3,5) - HK18*P(5,5);
-const float HK25 = HK12*P(2,3) + HK13*P(3,6) + HK14*P(0,3) - HK15*P(3,4) + HK16*P(1,3) - HK17*P(3,3) - HK18*P(3,5);
-const float HK26 = 1.0F/(HK12*HK21 + HK13*HK20 + HK14*HK19 - HK15*HK22 + HK16*HK23 - HK17*HK25 - HK18*HK24 + R_VEL);
+const float HK0 = q0*vn - q2*vd + q3*ve;
+const float HK1 = q1*vn + q2*ve + q3*vd;
+const float HK2 = q1*ve;
+const float HK3 = q0*vd;
+const float HK4 = q2*vn;
+const float HK5 = q0*ve + q1*vd - q3*vn;
+const float HK6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
+const float HK7 = q0*q3 + q1*q2;
+const float HK8 = q1*q3;
+const float HK9 = q0*q2;
+const float HK10 = 2*HK7;
+const float HK11 = -2*HK8 + 2*HK9;
+const float HK12 = 2*HK1;
+const float HK13 = 2*HK0;
+const float HK14 = -2*HK2 + 2*HK3 + 2*HK4;
+const float HK15 = 2*HK5;
+const float HK16 = HK10*P(0,5) - HK11*P(0,6) + HK12*P(0,1) + HK13*P(0,0) - HK14*P(0,2) + HK15*P(0,3) + HK6*P(0,4);
+const float HK17 = HK10*P(5,5) - HK11*P(5,6) + HK12*P(1,5) + HK13*P(0,5) - HK14*P(2,5) + HK15*P(3,5) + HK6*P(4,5);
+const float HK18 = HK10*P(5,6) - HK11*P(6,6) + HK12*P(1,6) + HK13*P(0,6) - HK14*P(2,6) + HK15*P(3,6) + HK6*P(4,6);
+const float HK19 = HK10*P(1,5) - HK11*P(1,6) + HK12*P(1,1) + HK13*P(0,1) - HK14*P(1,2) + HK15*P(1,3) + HK6*P(1,4);
+const float HK20 = HK10*P(2,5) - HK11*P(2,6) + HK12*P(1,2) + HK13*P(0,2) - HK14*P(2,2) + HK15*P(2,3) + HK6*P(2,4);
+const float HK21 = HK10*P(3,5) - HK11*P(3,6) + HK12*P(1,3) + HK13*P(0,3) - HK14*P(2,3) + HK15*P(3,3) + HK6*P(3,4);
+const float HK22 = HK10*P(4,5) - HK11*P(4,6) + HK12*P(1,4) + HK13*P(0,4) - HK14*P(2,4) + HK15*P(3,4) + HK6*P(4,4);
+const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK6 + R_VEL);
 
 
 H_VEL(0) = 2*HK0;
-H_VEL(1) = 2*HK2;
-H_VEL(2) = 2*HK3;
-H_VEL(3) = 2*HK4 - 2*HK5 - 2*HK6;
-H_VEL(4) = 2*HK7 - 2*HK8;
-H_VEL(5) = -HK10 - HK9 + 1;
-H_VEL(6) = 2*HK11;
+H_VEL(1) = 2*HK1;
+H_VEL(2) = 2*HK2 - 2*HK3 - 2*HK4;
+H_VEL(3) = 2*HK5;
+H_VEL(4) = HK6;
+H_VEL(5) = 2*HK7;
+H_VEL(6) = 2*HK8 - 2*HK9;
 H_VEL(7) = 0;
 H_VEL(8) = 0;
 H_VEL(9) = 0;
@@ -135,72 +49,151 @@ H_VEL(20) = 0;
 H_VEL(21) = 0;
 H_VEL(22) = 0;
 H_VEL(23) = 0;
+H_VEL(24) = 0;
 
 
-Kfusion(0) = HK19*HK26;
-Kfusion(1) = HK23*HK26;
-Kfusion(2) = HK21*HK26;
-Kfusion(3) = HK25*HK26;
-Kfusion(4) = HK22*HK26;
-Kfusion(5) = HK24*HK26;
-Kfusion(6) = HK20*HK26;
-Kfusion(7) = HK26*(HK12*P(2,7) + HK13*P(6,7) + HK14*P(0,7) - HK15*P(4,7) + HK16*P(1,7) - HK17*P(3,7) - HK18*P(5,7));
-Kfusion(8) = HK26*(HK12*P(2,8) + HK13*P(6,8) + HK14*P(0,8) - HK15*P(4,8) + HK16*P(1,8) - HK17*P(3,8) - HK18*P(5,8));
-Kfusion(9) = HK26*(HK12*P(2,9) + HK13*P(6,9) + HK14*P(0,9) - HK15*P(4,9) + HK16*P(1,9) - HK17*P(3,9) - HK18*P(5,9));
-Kfusion(10) = HK26*(HK12*P(2,10) + HK13*P(6,10) + HK14*P(0,10) - HK15*P(4,10) + HK16*P(1,10) - HK17*P(3,10) - HK18*P(5,10));
-Kfusion(11) = HK26*(HK12*P(2,11) + HK13*P(6,11) + HK14*P(0,11) - HK15*P(4,11) + HK16*P(1,11) - HK17*P(3,11) - HK18*P(5,11));
-Kfusion(12) = HK26*(HK12*P(2,12) + HK13*P(6,12) + HK14*P(0,12) - HK15*P(4,12) + HK16*P(1,12) - HK17*P(3,12) - HK18*P(5,12));
-Kfusion(13) = HK26*(HK12*P(2,13) + HK13*P(6,13) + HK14*P(0,13) - HK15*P(4,13) + HK16*P(1,13) - HK17*P(3,13) - HK18*P(5,13));
-Kfusion(14) = HK26*(HK12*P(2,14) + HK13*P(6,14) + HK14*P(0,14) - HK15*P(4,14) + HK16*P(1,14) - HK17*P(3,14) - HK18*P(5,14));
-Kfusion(15) = HK26*(HK12*P(2,15) + HK13*P(6,15) + HK14*P(0,15) - HK15*P(4,15) + HK16*P(1,15) - HK17*P(3,15) - HK18*P(5,15));
-Kfusion(16) = HK26*(HK12*P(2,16) + HK13*P(6,16) + HK14*P(0,16) - HK15*P(4,16) + HK16*P(1,16) - HK17*P(3,16) - HK18*P(5,16));
-Kfusion(17) = HK26*(HK12*P(2,17) + HK13*P(6,17) + HK14*P(0,17) - HK15*P(4,17) + HK16*P(1,17) - HK17*P(3,17) - HK18*P(5,17));
-Kfusion(18) = HK26*(HK12*P(2,18) + HK13*P(6,18) + HK14*P(0,18) - HK15*P(4,18) + HK16*P(1,18) - HK17*P(3,18) - HK18*P(5,18));
-Kfusion(19) = HK26*(HK12*P(2,19) + HK13*P(6,19) + HK14*P(0,19) - HK15*P(4,19) + HK16*P(1,19) - HK17*P(3,19) - HK18*P(5,19));
-Kfusion(20) = HK26*(HK12*P(2,20) + HK13*P(6,20) + HK14*P(0,20) - HK15*P(4,20) + HK16*P(1,20) - HK17*P(3,20) - HK18*P(5,20));
-Kfusion(21) = HK26*(HK12*P(2,21) + HK13*P(6,21) + HK14*P(0,21) - HK15*P(4,21) + HK16*P(1,21) - HK17*P(3,21) - HK18*P(5,21));
-Kfusion(22) = HK26*(HK12*P(2,22) + HK13*P(6,22) + HK14*P(0,22) - HK15*P(4,22) + HK16*P(1,22) - HK17*P(3,22) - HK18*P(5,22));
-Kfusion(23) = HK26*(HK12*P(2,23) + HK13*P(6,23) + HK14*P(0,23) - HK15*P(4,23) + HK16*P(1,23) - HK17*P(3,23) - HK18*P(5,23));
+Kfusion(0) = HK16*HK23;
+Kfusion(1) = HK19*HK23;
+Kfusion(2) = HK20*HK23;
+Kfusion(3) = HK21*HK23;
+Kfusion(4) = HK22*HK23;
+Kfusion(5) = HK17*HK23;
+Kfusion(6) = HK18*HK23;
+Kfusion(7) = HK23*(HK10*P(5,7) - HK11*P(6,7) + HK12*P(1,7) + HK13*P(0,7) - HK14*P(2,7) + HK15*P(3,7) + HK6*P(4,7));
+Kfusion(8) = HK23*(HK10*P(5,8) - HK11*P(6,8) + HK12*P(1,8) + HK13*P(0,8) - HK14*P(2,8) + HK15*P(3,8) + HK6*P(4,8));
+Kfusion(9) = HK23*(HK10*P(5,9) - HK11*P(6,9) + HK12*P(1,9) + HK13*P(0,9) - HK14*P(2,9) + HK15*P(3,9) + HK6*P(4,9));
+Kfusion(10) = HK23*(HK10*P(5,10) - HK11*P(6,10) + HK12*P(1,10) + HK13*P(0,10) - HK14*P(2,10) + HK15*P(3,10) + HK6*P(4,10));
+Kfusion(11) = HK23*(HK10*P(5,11) - HK11*P(6,11) + HK12*P(1,11) + HK13*P(0,11) - HK14*P(2,11) + HK15*P(3,11) + HK6*P(4,11));
+Kfusion(12) = HK23*(HK10*P(5,12) - HK11*P(6,12) + HK12*P(1,12) + HK13*P(0,12) - HK14*P(2,12) + HK15*P(3,12) + HK6*P(4,12));
+Kfusion(13) = HK23*(HK10*P(5,13) - HK11*P(6,13) + HK12*P(1,13) + HK13*P(0,13) - HK14*P(2,13) + HK15*P(3,13) + HK6*P(4,13));
+Kfusion(14) = HK23*(HK10*P(5,14) - HK11*P(6,14) + HK12*P(1,14) + HK13*P(0,14) - HK14*P(2,14) + HK15*P(3,14) + HK6*P(4,14));
+Kfusion(15) = HK23*(HK10*P(5,15) - HK11*P(6,15) + HK12*P(1,15) + HK13*P(0,15) - HK14*P(2,15) + HK15*P(3,15) + HK6*P(4,15));
+Kfusion(16) = HK23*(HK10*P(5,16) - HK11*P(6,16) + HK12*P(1,16) + HK13*P(0,16) - HK14*P(2,16) + HK15*P(3,16) + HK6*P(4,16));
+Kfusion(17) = HK23*(HK10*P(5,17) - HK11*P(6,17) + HK12*P(1,17) + HK13*P(0,17) - HK14*P(2,17) + HK15*P(3,17) + HK6*P(4,17));
+Kfusion(18) = HK23*(HK10*P(5,18) - HK11*P(6,18) + HK12*P(1,18) + HK13*P(0,18) - HK14*P(2,18) + HK15*P(3,18) + HK6*P(4,18));
+Kfusion(19) = HK23*(HK10*P(5,19) - HK11*P(6,19) + HK12*P(1,19) + HK13*P(0,19) - HK14*P(2,19) + HK15*P(3,19) + HK6*P(4,19));
+Kfusion(20) = HK23*(HK10*P(5,20) - HK11*P(6,20) + HK12*P(1,20) + HK13*P(0,20) - HK14*P(2,20) + HK15*P(3,20) + HK6*P(4,20));
+Kfusion(21) = HK23*(HK10*P(5,21) - HK11*P(6,21) + HK12*P(1,21) + HK13*P(0,21) - HK14*P(2,21) + HK15*P(3,21) + HK6*P(4,21));
+Kfusion(22) = HK23*(HK10*P(5,22) - HK11*P(6,22) + HK12*P(1,22) + HK13*P(0,22) - HK14*P(2,22) + HK15*P(3,22) + HK6*P(4,22));
+Kfusion(23) = HK23*(HK10*P(5,23) - HK11*P(6,23) + HK12*P(1,23) + HK13*P(0,23) - HK14*P(2,23) + HK15*P(3,23) + HK6*P(4,23));
+Kfusion(24) = HK23*(HK10*P(5,24) - HK11*P(6,24) + HK12*P(1,24) + HK13*P(0,24) - HK14*P(2,24) + HK15*P(3,24) + HK6*P(4,24));
+
+
+// axis 1
+const float HK0 = q0*ve + q1*vd - q3*vn;
+const float HK1 = q0*vd - q1*ve + q2*vn;
+const float HK2 = q1*vn + q2*ve + q3*vd;
+const float HK3 = q2*vd;
+const float HK4 = q0*vn;
+const float HK5 = q3*ve;
+const float HK6 = q1*q2;
+const float HK7 = q0*q3;
+const float HK8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
+const float HK9 = q0*q1 + q2*q3;
+const float HK10 = 2*HK9;
+const float HK11 = -2*HK6 + 2*HK7;
+const float HK12 = 2*HK2;
+const float HK13 = 2*HK0;
+const float HK14 = 2*HK1;
+const float HK15 = -2*HK3 + 2*HK4 + 2*HK5;
+const float HK16 = HK10*P(0,6) - HK11*P(0,4) + HK12*P(0,2) + HK13*P(0,0) + HK14*P(0,1) - HK15*P(0,3) + HK8*P(0,5);
+const float HK17 = HK10*P(6,6) - HK11*P(4,6) + HK12*P(2,6) + HK13*P(0,6) + HK14*P(1,6) - HK15*P(3,6) + HK8*P(5,6);
+const float HK18 = HK10*P(4,6) - HK11*P(4,4) + HK12*P(2,4) + HK13*P(0,4) + HK14*P(1,4) - HK15*P(3,4) + HK8*P(4,5);
+const float HK19 = HK10*P(2,6) - HK11*P(2,4) + HK12*P(2,2) + HK13*P(0,2) + HK14*P(1,2) - HK15*P(2,3) + HK8*P(2,5);
+const float HK20 = HK10*P(1,6) - HK11*P(1,4) + HK12*P(1,2) + HK13*P(0,1) + HK14*P(1,1) - HK15*P(1,3) + HK8*P(1,5);
+const float HK21 = HK10*P(3,6) - HK11*P(3,4) + HK12*P(2,3) + HK13*P(0,3) + HK14*P(1,3) - HK15*P(3,3) + HK8*P(3,5);
+const float HK22 = HK10*P(5,6) - HK11*P(4,5) + HK12*P(2,5) + HK13*P(0,5) + HK14*P(1,5) - HK15*P(3,5) + HK8*P(5,5);
+const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 + HK14*HK20 - HK15*HK21 + HK22*HK8 + R_VEL);
+
+
+H_VEL(0) = 2*HK0;
+H_VEL(1) = 2*HK1;
+H_VEL(2) = 2*HK2;
+H_VEL(3) = 2*HK3 - 2*HK4 - 2*HK5;
+H_VEL(4) = 2*HK6 - 2*HK7;
+H_VEL(5) = HK8;
+H_VEL(6) = 2*HK9;
+H_VEL(7) = 0;
+H_VEL(8) = 0;
+H_VEL(9) = 0;
+H_VEL(10) = 0;
+H_VEL(11) = 0;
+H_VEL(12) = 0;
+H_VEL(13) = 0;
+H_VEL(14) = 0;
+H_VEL(15) = 0;
+H_VEL(16) = 0;
+H_VEL(17) = 0;
+H_VEL(18) = 0;
+H_VEL(19) = 0;
+H_VEL(20) = 0;
+H_VEL(21) = 0;
+H_VEL(22) = 0;
+H_VEL(23) = 0;
+H_VEL(24) = 0;
+
+
+Kfusion(0) = HK16*HK23;
+Kfusion(1) = HK20*HK23;
+Kfusion(2) = HK19*HK23;
+Kfusion(3) = HK21*HK23;
+Kfusion(4) = HK18*HK23;
+Kfusion(5) = HK22*HK23;
+Kfusion(6) = HK17*HK23;
+Kfusion(7) = HK23*(HK10*P(6,7) - HK11*P(4,7) + HK12*P(2,7) + HK13*P(0,7) + HK14*P(1,7) - HK15*P(3,7) + HK8*P(5,7));
+Kfusion(8) = HK23*(HK10*P(6,8) - HK11*P(4,8) + HK12*P(2,8) + HK13*P(0,8) + HK14*P(1,8) - HK15*P(3,8) + HK8*P(5,8));
+Kfusion(9) = HK23*(HK10*P(6,9) - HK11*P(4,9) + HK12*P(2,9) + HK13*P(0,9) + HK14*P(1,9) - HK15*P(3,9) + HK8*P(5,9));
+Kfusion(10) = HK23*(HK10*P(6,10) - HK11*P(4,10) + HK12*P(2,10) + HK13*P(0,10) + HK14*P(1,10) - HK15*P(3,10) + HK8*P(5,10));
+Kfusion(11) = HK23*(HK10*P(6,11) - HK11*P(4,11) + HK12*P(2,11) + HK13*P(0,11) + HK14*P(1,11) - HK15*P(3,11) + HK8*P(5,11));
+Kfusion(12) = HK23*(HK10*P(6,12) - HK11*P(4,12) + HK12*P(2,12) + HK13*P(0,12) + HK14*P(1,12) - HK15*P(3,12) + HK8*P(5,12));
+Kfusion(13) = HK23*(HK10*P(6,13) - HK11*P(4,13) + HK12*P(2,13) + HK13*P(0,13) + HK14*P(1,13) - HK15*P(3,13) + HK8*P(5,13));
+Kfusion(14) = HK23*(HK10*P(6,14) - HK11*P(4,14) + HK12*P(2,14) + HK13*P(0,14) + HK14*P(1,14) - HK15*P(3,14) + HK8*P(5,14));
+Kfusion(15) = HK23*(HK10*P(6,15) - HK11*P(4,15) + HK12*P(2,15) + HK13*P(0,15) + HK14*P(1,15) - HK15*P(3,15) + HK8*P(5,15));
+Kfusion(16) = HK23*(HK10*P(6,16) - HK11*P(4,16) + HK12*P(2,16) + HK13*P(0,16) + HK14*P(1,16) - HK15*P(3,16) + HK8*P(5,16));
+Kfusion(17) = HK23*(HK10*P(6,17) - HK11*P(4,17) + HK12*P(2,17) + HK13*P(0,17) + HK14*P(1,17) - HK15*P(3,17) + HK8*P(5,17));
+Kfusion(18) = HK23*(HK10*P(6,18) - HK11*P(4,18) + HK12*P(2,18) + HK13*P(0,18) + HK14*P(1,18) - HK15*P(3,18) + HK8*P(5,18));
+Kfusion(19) = HK23*(HK10*P(6,19) - HK11*P(4,19) + HK12*P(2,19) + HK13*P(0,19) + HK14*P(1,19) - HK15*P(3,19) + HK8*P(5,19));
+Kfusion(20) = HK23*(HK10*P(6,20) - HK11*P(4,20) + HK12*P(2,20) + HK13*P(0,20) + HK14*P(1,20) - HK15*P(3,20) + HK8*P(5,20));
+Kfusion(21) = HK23*(HK10*P(6,21) - HK11*P(4,21) + HK12*P(2,21) + HK13*P(0,21) + HK14*P(1,21) - HK15*P(3,21) + HK8*P(5,21));
+Kfusion(22) = HK23*(HK10*P(6,22) - HK11*P(4,22) + HK12*P(2,22) + HK13*P(0,22) + HK14*P(1,22) - HK15*P(3,22) + HK8*P(5,22));
+Kfusion(23) = HK23*(HK10*P(6,23) - HK11*P(4,23) + HK12*P(2,23) + HK13*P(0,23) + HK14*P(1,23) - HK15*P(3,23) + HK8*P(5,23));
+Kfusion(24) = HK23*(HK10*P(6,24) - HK11*P(4,24) + HK12*P(2,24) + HK13*P(0,24) + HK14*P(1,24) - HK15*P(3,24) + HK8*P(5,24));
 
 
 // axis 2
-const float HK0 = q2*vn;
-const float HK1 = q1*ve;
-const float HK2 = q3*vn;
-const float HK3 = q0*ve;
-const float HK4 = 2*vd;
-const float HK5 = HK4*q1;
-const float HK6 = -HK4*q2 + q0*vn + q3*ve;
-const float HK7 = q1*vn + q2*ve;
-const float HK8 = q0*q2 + q1*q3;
-const float HK9 = q2*q3;
-const float HK10 = q0*q1;
-const float HK11 = 2*powf(q1, 2);
-const float HK12 = 2*powf(q2, 2);
-const float HK13 = 2*HK7;
-const float HK14 = 2*HK8;
-const float HK15 = -2*HK0 + 2*HK1;
-const float HK16 = 2*HK10 - 2*HK9;
-const float HK17 = 2*HK6;
-const float HK18 = HK11 + HK12 - 1;
-const float HK19 = -2*HK2 + 2*HK3 + 2*HK5;
-const float HK20 = -HK13*P(0,3) - HK14*P(0,4) + HK15*P(0,0) + HK16*P(0,5) - HK17*P(0,2) + HK18*P(0,6) + HK19*P(0,1);
-const float HK21 = -HK13*P(3,4) - HK14*P(4,4) + HK15*P(0,4) + HK16*P(4,5) - HK17*P(2,4) + HK18*P(4,6) + HK19*P(1,4);
-const float HK22 = -HK13*P(3,3) - HK14*P(3,4) + HK15*P(0,3) + HK16*P(3,5) - HK17*P(2,3) + HK18*P(3,6) + HK19*P(1,3);
-const float HK23 = -HK13*P(3,5) - HK14*P(4,5) + HK15*P(0,5) + HK16*P(5,5) - HK17*P(2,5) + HK18*P(5,6) + HK19*P(1,5);
-const float HK24 = -HK13*P(3,6) - HK14*P(4,6) + HK15*P(0,6) + HK16*P(5,6) - HK17*P(2,6) + HK18*P(6,6) + HK19*P(1,6);
-const float HK25 = -HK13*P(2,3) - HK14*P(2,4) + HK15*P(0,2) + HK16*P(2,5) - HK17*P(2,2) + HK18*P(2,6) + HK19*P(1,2);
-const float HK26 = -HK13*P(1,3) - HK14*P(1,4) + HK15*P(0,1) + HK16*P(1,5) - HK17*P(1,2) + HK18*P(1,6) + HK19*P(1,1);
-const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL);
+const float HK0 = q0*vd - q1*ve + q2*vn;
+const float HK1 = q3*vn;
+const float HK2 = q0*ve;
+const float HK3 = q1*vd;
+const float HK4 = q0*vn - q2*vd + q3*ve;
+const float HK5 = q1*vn + q2*ve + q3*vd;
+const float HK6 = q0*q2 + q1*q3;
+const float HK7 = q2*q3;
+const float HK8 = q0*q1;
+const float HK9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
+const float HK10 = 2*HK6;
+const float HK11 = -2*HK7 + 2*HK8;
+const float HK12 = 2*HK5;
+const float HK13 = 2*HK0;
+const float HK14 = -2*HK1 + 2*HK2 + 2*HK3;
+const float HK15 = 2*HK4;
+const float HK16 = HK10*P(0,4) - HK11*P(0,5) + HK12*P(0,3) + HK13*P(0,0) - HK14*P(0,1) + HK15*P(0,2) + HK9*P(0,6);
+const float HK17 = HK10*P(4,4) - HK11*P(4,5) + HK12*P(3,4) + HK13*P(0,4) - HK14*P(1,4) + HK15*P(2,4) + HK9*P(4,6);
+const float HK18 = HK10*P(4,5) - HK11*P(5,5) + HK12*P(3,5) + HK13*P(0,5) - HK14*P(1,5) + HK15*P(2,5) + HK9*P(5,6);
+const float HK19 = HK10*P(3,4) - HK11*P(3,5) + HK12*P(3,3) + HK13*P(0,3) - HK14*P(1,3) + HK15*P(2,3) + HK9*P(3,6);
+const float HK20 = HK10*P(1,4) - HK11*P(1,5) + HK12*P(1,3) + HK13*P(0,1) - HK14*P(1,1) + HK15*P(1,2) + HK9*P(1,6);
+const float HK21 = HK10*P(2,4) - HK11*P(2,5) + HK12*P(2,3) + HK13*P(0,2) - HK14*P(1,2) + HK15*P(2,2) + HK9*P(2,6);
+const float HK22 = HK10*P(4,6) - HK11*P(5,6) + HK12*P(3,6) + HK13*P(0,6) - HK14*P(1,6) + HK15*P(2,6) + HK9*P(6,6);
+const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK9 + R_VEL);
 
 
-H_VEL(0) = 2*HK0 - 2*HK1;
-H_VEL(1) = 2*HK2 - 2*HK3 - 2*HK5;
-H_VEL(2) = 2*HK6;
-H_VEL(3) = 2*HK7;
-H_VEL(4) = 2*HK8;
-H_VEL(5) = -2*HK10 + 2*HK9;
-H_VEL(6) = -HK11 - HK12 + 1;
+H_VEL(0) = 2*HK0;
+H_VEL(1) = 2*HK1 - 2*HK2 - 2*HK3;
+H_VEL(2) = 2*HK4;
+H_VEL(3) = 2*HK5;
+H_VEL(4) = 2*HK6;
+H_VEL(5) = 2*HK7 - 2*HK8;
+H_VEL(6) = HK9;
 H_VEL(7) = 0;
 H_VEL(8) = 0;
 H_VEL(9) = 0;
@@ -218,29 +211,31 @@ H_VEL(20) = 0;
 H_VEL(21) = 0;
 H_VEL(22) = 0;
 H_VEL(23) = 0;
+H_VEL(24) = 0;
 
 
-Kfusion(0) = -HK20*HK27;
-Kfusion(1) = -HK26*HK27;
-Kfusion(2) = -HK25*HK27;
-Kfusion(3) = -HK22*HK27;
-Kfusion(4) = -HK21*HK27;
-Kfusion(5) = -HK23*HK27;
-Kfusion(6) = -HK24*HK27;
-Kfusion(7) = -HK27*(-HK13*P(3,7) - HK14*P(4,7) + HK15*P(0,7) + HK16*P(5,7) - HK17*P(2,7) + HK18*P(6,7) + HK19*P(1,7));
-Kfusion(8) = -HK27*(-HK13*P(3,8) - HK14*P(4,8) + HK15*P(0,8) + HK16*P(5,8) - HK17*P(2,8) + HK18*P(6,8) + HK19*P(1,8));
-Kfusion(9) = -HK27*(-HK13*P(3,9) - HK14*P(4,9) + HK15*P(0,9) + HK16*P(5,9) - HK17*P(2,9) + HK18*P(6,9) + HK19*P(1,9));
-Kfusion(10) = -HK27*(-HK13*P(3,10) - HK14*P(4,10) + HK15*P(0,10) + HK16*P(5,10) - HK17*P(2,10) + HK18*P(6,10) + HK19*P(1,10));
-Kfusion(11) = -HK27*(-HK13*P(3,11) - HK14*P(4,11) + HK15*P(0,11) + HK16*P(5,11) - HK17*P(2,11) + HK18*P(6,11) + HK19*P(1,11));
-Kfusion(12) = -HK27*(-HK13*P(3,12) - HK14*P(4,12) + HK15*P(0,12) + HK16*P(5,12) - HK17*P(2,12) + HK18*P(6,12) + HK19*P(1,12));
-Kfusion(13) = -HK27*(-HK13*P(3,13) - HK14*P(4,13) + HK15*P(0,13) + HK16*P(5,13) - HK17*P(2,13) + HK18*P(6,13) + HK19*P(1,13));
-Kfusion(14) = -HK27*(-HK13*P(3,14) - HK14*P(4,14) + HK15*P(0,14) + HK16*P(5,14) - HK17*P(2,14) + HK18*P(6,14) + HK19*P(1,14));
-Kfusion(15) = -HK27*(-HK13*P(3,15) - HK14*P(4,15) + HK15*P(0,15) + HK16*P(5,15) - HK17*P(2,15) + HK18*P(6,15) + HK19*P(1,15));
-Kfusion(16) = -HK27*(-HK13*P(3,16) - HK14*P(4,16) + HK15*P(0,16) + HK16*P(5,16) - HK17*P(2,16) + HK18*P(6,16) + HK19*P(1,16));
-Kfusion(17) = -HK27*(-HK13*P(3,17) - HK14*P(4,17) + HK15*P(0,17) + HK16*P(5,17) - HK17*P(2,17) + HK18*P(6,17) + HK19*P(1,17));
-Kfusion(18) = -HK27*(-HK13*P(3,18) - HK14*P(4,18) + HK15*P(0,18) + HK16*P(5,18) - HK17*P(2,18) + HK18*P(6,18) + HK19*P(1,18));
-Kfusion(19) = -HK27*(-HK13*P(3,19) - HK14*P(4,19) + HK15*P(0,19) + HK16*P(5,19) - HK17*P(2,19) + HK18*P(6,19) + HK19*P(1,19));
-Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20) - HK17*P(2,20) + HK18*P(6,20) + HK19*P(1,20));
-Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21));
-Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22));
-Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23));
+Kfusion(0) = HK16*HK23;
+Kfusion(1) = HK20*HK23;
+Kfusion(2) = HK21*HK23;
+Kfusion(3) = HK19*HK23;
+Kfusion(4) = HK17*HK23;
+Kfusion(5) = HK18*HK23;
+Kfusion(6) = HK22*HK23;
+Kfusion(7) = HK23*(HK10*P(4,7) - HK11*P(5,7) + HK12*P(3,7) + HK13*P(0,7) - HK14*P(1,7) + HK15*P(2,7) + HK9*P(6,7));
+Kfusion(8) = HK23*(HK10*P(4,8) - HK11*P(5,8) + HK12*P(3,8) + HK13*P(0,8) - HK14*P(1,8) + HK15*P(2,8) + HK9*P(6,8));
+Kfusion(9) = HK23*(HK10*P(4,9) - HK11*P(5,9) + HK12*P(3,9) + HK13*P(0,9) - HK14*P(1,9) + HK15*P(2,9) + HK9*P(6,9));
+Kfusion(10) = HK23*(HK10*P(4,10) - HK11*P(5,10) + HK12*P(3,10) + HK13*P(0,10) - HK14*P(1,10) + HK15*P(2,10) + HK9*P(6,10));
+Kfusion(11) = HK23*(HK10*P(4,11) - HK11*P(5,11) + HK12*P(3,11) + HK13*P(0,11) - HK14*P(1,11) + HK15*P(2,11) + HK9*P(6,11));
+Kfusion(12) = HK23*(HK10*P(4,12) - HK11*P(5,12) + HK12*P(3,12) + HK13*P(0,12) - HK14*P(1,12) + HK15*P(2,12) + HK9*P(6,12));
+Kfusion(13) = HK23*(HK10*P(4,13) - HK11*P(5,13) + HK12*P(3,13) + HK13*P(0,13) - HK14*P(1,13) + HK15*P(2,13) + HK9*P(6,13));
+Kfusion(14) = HK23*(HK10*P(4,14) - HK11*P(5,14) + HK12*P(3,14) + HK13*P(0,14) - HK14*P(1,14) + HK15*P(2,14) + HK9*P(6,14));
+Kfusion(15) = HK23*(HK10*P(4,15) - HK11*P(5,15) + HK12*P(3,15) + HK13*P(0,15) - HK14*P(1,15) + HK15*P(2,15) + HK9*P(6,15));
+Kfusion(16) = HK23*(HK10*P(4,16) - HK11*P(5,16) + HK12*P(3,16) + HK13*P(0,16) - HK14*P(1,16) + HK15*P(2,16) + HK9*P(6,16));
+Kfusion(17) = HK23*(HK10*P(4,17) - HK11*P(5,17) + HK12*P(3,17) + HK13*P(0,17) - HK14*P(1,17) + HK15*P(2,17) + HK9*P(6,17));
+Kfusion(18) = HK23*(HK10*P(4,18) - HK11*P(5,18) + HK12*P(3,18) + HK13*P(0,18) - HK14*P(1,18) + HK15*P(2,18) + HK9*P(6,18));
+Kfusion(19) = HK23*(HK10*P(4,19) - HK11*P(5,19) + HK12*P(3,19) + HK13*P(0,19) - HK14*P(1,19) + HK15*P(2,19) + HK9*P(6,19));
+Kfusion(20) = HK23*(HK10*P(4,20) - HK11*P(5,20) + HK12*P(3,20) + HK13*P(0,20) - HK14*P(1,20) + HK15*P(2,20) + HK9*P(6,20));
+Kfusion(21) = HK23*(HK10*P(4,21) - HK11*P(5,21) + HK12*P(3,21) + HK13*P(0,21) - HK14*P(1,21) + HK15*P(2,21) + HK9*P(6,21));
+Kfusion(22) = HK23*(HK10*P(4,22) - HK11*P(5,22) + HK12*P(3,22) + HK13*P(0,22) - HK14*P(1,22) + HK15*P(2,22) + HK9*P(6,22));
+Kfusion(23) = HK23*(HK10*P(4,23) - HK11*P(5,23) + HK12*P(3,23) + HK13*P(0,23) - HK14*P(1,23) + HK15*P(2,23) + HK9*P(6,23));
+Kfusion(24) = HK23*(HK10*P(4,24) - HK11*P(5,24) + HK12*P(3,24) + HK13*P(0,24) - HK14*P(1,24) + HK15*P(2,24) + HK9*P(6,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp

@@ -1,100 +1,96 @@
 // Sub Expressions
-const float HK0 = q3*ve;
-const float HK1 = q2*vd;
-const float HK2 = -HK1;
-const float HK3 = q2*ve;
-const float HK4 = q3*vd;
-const float HK5 = HK3 + HK4;
-const float HK6 = q1*ve;
-const float HK7 = q0*vd;
-const float HK8 = q2*vn;
-const float HK9 = 2*HK8;
+const float HK0 = q0*vn;
+const float HK1 = q3*ve;
+const float HK2 = q2*vd;
+const float HK3 = HK0 + HK1 - HK2;
+const float HK4 = 2*HK3;
+const float HK5 = q1*vn + q2*ve + q3*vd;
+const float HK6 = 2*HK5;
+const float HK7 = q1*ve;
+const float HK8 = q0*vd;
+const float HK9 = q2*vn;
 const float HK10 = q0*ve;
 const float HK11 = q1*vd;
 const float HK12 = q3*vn;
-const float HK13 = HK10 + HK11 - 2*HK12;
-const float HK14 = 2*powf(q2, 2);
-const float HK15 = -HK14;
-const float HK16 = 2*powf(q3, 2);
-const float HK17 = 1 - HK16;
-const float HK18 = q0*q3;
-const float HK19 = q1*q2;
-const float HK20 = HK18 + HK19;
-const float HK21 = q1*q3;
-const float HK22 = q0*q2;
-const float HK23 = 2*HK5;
-const float HK24 = 2*HK20;
-const float HK25 = -2*HK0 + 2*HK1;
-const float HK26 = -2*HK21 + 2*HK22;
-const float HK27 = 2*HK13;
-const float HK28 = HK16 - 1;
-const float HK29 = HK14 + HK28;
-const float HK30 = -HK6;
-const float HK31 = 2*HK30 + 2*HK7 + 2*HK9;
-const float HK32 = -HK23*P(0,1) - HK24*P(0,5) + HK25*P(0,0) + HK26*P(0,6) - HK27*P(0,3) + HK29*P(0,4) + HK31*P(0,2);
-const float HK33 = -HK23*P(1,5) - HK24*P(5,5) + HK25*P(0,5) + HK26*P(5,6) - HK27*P(3,5) + HK29*P(4,5) + HK31*P(2,5);
-const float HK34 = -HK23*P(1,1) - HK24*P(1,5) + HK25*P(0,1) + HK26*P(1,6) - HK27*P(1,3) + HK29*P(1,4) + HK31*P(1,2);
-const float HK35 = -HK23*P(1,6) - HK24*P(5,6) + HK25*P(0,6) + HK26*P(6,6) - HK27*P(3,6) + HK29*P(4,6) + HK31*P(2,6);
-const float HK36 = -HK23*P(1,4) - HK24*P(4,5) + HK25*P(0,4) + HK26*P(4,6) - HK27*P(3,4) + HK29*P(4,4) + HK31*P(2,4);
-const float HK37 = -HK23*P(1,3) - HK24*P(3,5) + HK25*P(0,3) + HK26*P(3,6) - HK27*P(3,3) + HK29*P(3,4) + HK31*P(2,3);
-const float HK38 = -HK23*P(1,2) - HK24*P(2,5) + HK25*P(0,2) + HK26*P(2,6) - HK27*P(2,3) + HK29*P(2,4) + HK31*P(2,2);
-const float HK39 = 1.0F/(-HK23*HK34 - HK24*HK33 + HK25*HK32 + HK26*HK35 - HK27*HK37 + HK29*HK36 + HK31*HK38 + R_VEL);
-const float HK40 = -HK12;
-const float HK41 = HK11 + HK40;
-const float HK42 = -2*HK6 + HK7 + HK8;
-const float HK43 = q1*vn;
-const float HK44 = HK4 + HK43;
-const float HK45 = q0*vn;
-const float HK46 = 2*HK0;
-const float HK47 = 2*powf(q1, 2);
-const float HK48 = -HK47;
-const float HK49 = q0*q1;
-const float HK50 = q2*q3;
-const float HK51 = HK49 + HK50;
-const float HK52 = 2*HK44;
-const float HK53 = 2*HK51;
-const float HK54 = 2*HK41;
-const float HK55 = 2*HK18 - 2*HK19;
-const float HK56 = 2*HK42;
-const float HK57 = 2*HK2 + 2*HK45 + 2*HK46;
-const float HK58 = HK28 + HK47;
-const float HK59 = HK52*P(0,2) + HK53*P(0,6) + HK54*P(0,0) - HK55*P(0,4) + HK56*P(0,1) - HK57*P(0,3) - HK58*P(0,5);
-const float HK60 = HK52*P(2,6) + HK53*P(6,6) + HK54*P(0,6) - HK55*P(4,6) + HK56*P(1,6) - HK57*P(3,6) - HK58*P(5,6);
-const float HK61 = HK52*P(2,2) + HK53*P(2,6) + HK54*P(0,2) - HK55*P(2,4) + HK56*P(1,2) - HK57*P(2,3) - HK58*P(2,5);
-const float HK62 = HK52*P(2,4) + HK53*P(4,6) + HK54*P(0,4) - HK55*P(4,4) + HK56*P(1,4) - HK57*P(3,4) - HK58*P(4,5);
-const float HK63 = HK52*P(1,2) + HK53*P(1,6) + HK54*P(0,1) - HK55*P(1,4) + HK56*P(1,1) - HK57*P(1,3) - HK58*P(1,5);
-const float HK64 = HK52*P(2,5) + HK53*P(5,6) + HK54*P(0,5) - HK55*P(4,5) + HK56*P(1,5) - HK57*P(3,5) - HK58*P(5,5);
-const float HK65 = HK52*P(2,3) + HK53*P(3,6) + HK54*P(0,3) - HK55*P(3,4) + HK56*P(1,3) - HK57*P(3,3) - HK58*P(3,5);
-const float HK66 = 1.0F/(HK52*HK61 + HK53*HK60 + HK54*HK59 - HK55*HK62 + HK56*HK63 - HK57*HK65 - HK58*HK64 + R_VEL);
-const float HK67 = 2*HK11;
-const float HK68 = HK0 - 2*HK1 + HK45;
-const float HK69 = HK3 + HK43;
-const float HK70 = HK21 + HK22;
-const float HK71 = 2*HK69;
+const float HK13 = HK10 + HK11 - HK12;
+const float HK14 = 2*HK13;
+const float HK15 = powf(q1, 2);
+const float HK16 = powf(q2, 2);
+const float HK17 = -HK16;
+const float HK18 = powf(q0, 2);
+const float HK19 = powf(q3, 2);
+const float HK20 = HK18 - HK19;
+const float HK21 = HK15 + HK17 + HK20;
+const float HK22 = q0*q3;
+const float HK23 = q1*q2;
+const float HK24 = HK22 + HK23;
+const float HK25 = q1*q3;
+const float HK26 = q0*q2;
+const float HK27 = 2*HK24;
+const float HK28 = -2*HK25 + 2*HK26;
+const float HK29 = 2*HK5;
+const float HK30 = 2*HK3;
+const float HK31 = -HK7 + HK8 + HK9;
+const float HK32 = 2*HK31;
+const float HK33 = HK32*P(0,2);
+const float HK34 = 2*HK13;
+const float HK35 = HK34*P(0,3);
+const float HK36 = HK21*P(0,4) + HK27*P(0,5) - HK28*P(0,6) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35;
+const float HK37 = HK21*P(4,5) + HK27*P(5,5) - HK28*P(5,6) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5);
+const float HK38 = HK21*P(4,6) + HK27*P(5,6) - HK28*P(6,6) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6);
+const float HK39 = HK32*P(1,2);
+const float HK40 = HK34*P(1,3);
+const float HK41 = HK21*P(1,4) + HK27*P(1,5) - HK28*P(1,6) + HK29*P(1,1) + HK30*P(0,1) - HK39 + HK40;
+const float HK42 = HK29*P(1,2);
+const float HK43 = HK30*P(0,2);
+const float HK44 = HK21*P(2,4) + HK27*P(2,5) - HK28*P(2,6) - HK32*P(2,2) + HK34*P(2,3) + HK42 + HK43;
+const float HK45 = HK29*P(1,3);
+const float HK46 = HK30*P(0,3);
+const float HK47 = HK21*P(3,4) + HK27*P(3,5) - HK28*P(3,6) - HK32*P(2,3) + HK34*P(3,3) + HK45 + HK46;
+const float HK48 = HK21*P(4,4) + HK27*P(4,5) - HK28*P(4,6) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4);
+const float HK49 = 1.0F/(HK21*HK48 + HK27*HK37 - HK28*HK38 + HK29*HK41 + HK30*HK36 - HK32*HK44 + HK34*HK47 + R_VEL);
+const float HK50 = 2*HK31;
+const float HK51 = -HK15;
+const float HK52 = HK16 + HK20 + HK51;
+const float HK53 = q0*q1;
+const float HK54 = q2*q3;
+const float HK55 = HK53 + HK54;
+const float HK56 = 2*HK55;
+const float HK57 = 2*HK22 - 2*HK23;
+const float HK58 = HK32*P(0,1);
+const float HK59 = HK29*P(0,2) + HK34*P(0,0) - HK46 + HK52*P(0,5) + HK56*P(0,6) - HK57*P(0,4) + HK58;
+const float HK60 = HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK52*P(5,6) + HK56*P(6,6) - HK57*P(4,6);
+const float HK61 = HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK52*P(4,5) + HK56*P(4,6) - HK57*P(4,4);
+const float HK62 = HK30*P(2,3);
+const float HK63 = HK29*P(2,2) + HK34*P(0,2) + HK39 + HK52*P(2,5) + HK56*P(2,6) - HK57*P(2,4) - HK62;
+const float HK64 = HK34*P(0,1);
+const float HK65 = -HK30*P(1,3) + HK32*P(1,1) + HK42 + HK52*P(1,5) + HK56*P(1,6) - HK57*P(1,4) + HK64;
+const float HK66 = HK29*P(2,3);
+const float HK67 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK52*P(3,5) + HK56*P(3,6) - HK57*P(3,4) + HK66;
+const float HK68 = HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK52*P(5,5) + HK56*P(5,6) - HK57*P(4,5);
+const float HK69 = 1.0F/(HK29*HK63 - HK30*HK67 + HK32*HK65 + HK34*HK59 + HK52*HK68 + HK56*HK60 - HK57*HK61 + R_VEL);
+const float HK70 = HK25 + HK26;
+const float HK71 = HK17 + HK18 + HK19 + HK51;
 const float HK72 = 2*HK70;
-const float HK73 = 2*HK6 - 2*HK8;
-const float HK74 = 2*HK49 - 2*HK50;
-const float HK75 = 2*HK68;
-const float HK76 = HK14 + HK47 - 1;
-const float HK77 = 2*HK10 + 2*HK40 + 2*HK67;
-const float HK78 = -HK71*P(0,3) - HK72*P(0,4) + HK73*P(0,0) + HK74*P(0,5) - HK75*P(0,2) + HK76*P(0,6) + HK77*P(0,1);
-const float HK79 = -HK71*P(3,4) - HK72*P(4,4) + HK73*P(0,4) + HK74*P(4,5) - HK75*P(2,4) + HK76*P(4,6) + HK77*P(1,4);
-const float HK80 = -HK71*P(3,3) - HK72*P(3,4) + HK73*P(0,3) + HK74*P(3,5) - HK75*P(2,3) + HK76*P(3,6) + HK77*P(1,3);
-const float HK81 = -HK71*P(3,5) - HK72*P(4,5) + HK73*P(0,5) + HK74*P(5,5) - HK75*P(2,5) + HK76*P(5,6) + HK77*P(1,5);
-const float HK82 = -HK71*P(3,6) - HK72*P(4,6) + HK73*P(0,6) + HK74*P(5,6) - HK75*P(2,6) + HK76*P(6,6) + HK77*P(1,6);
-const float HK83 = -HK71*P(2,3) - HK72*P(2,4) + HK73*P(0,2) + HK74*P(2,5) - HK75*P(2,2) + HK76*P(2,6) + HK77*P(1,2);
-const float HK84 = -HK71*P(1,3) - HK72*P(1,4) + HK73*P(0,1) + HK74*P(1,5) - HK75*P(1,2) + HK76*P(1,6) + HK77*P(1,1);
-const float HK85 = 1.0F/(-HK71*HK80 - HK72*HK79 + HK73*HK78 + HK74*HK81 - HK75*HK83 + HK76*HK82 + HK77*HK84 + R_VEL);
+const float HK73 = 2*HK53 - 2*HK54;
+const float HK74 = HK29*P(0,3) + HK32*P(0,0) + HK43 - HK64 + HK71*P(0,6) + HK72*P(0,4) - HK73*P(0,5);
+const float HK75 = HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK71*P(4,6) + HK72*P(4,4) - HK73*P(4,5);
+const float HK76 = HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK71*P(5,6) + HK72*P(4,5) - HK73*P(5,5);
+const float HK77 = HK29*P(3,3) + HK32*P(0,3) - HK40 + HK62 + HK71*P(3,6) + HK72*P(3,4) - HK73*P(3,5);
+const float HK78 = HK30*P(1,2) - HK34*P(1,1) + HK45 + HK58 + HK71*P(1,6) + HK72*P(1,4) - HK73*P(1,5);
+const float HK79 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK66 + HK71*P(2,6) + HK72*P(2,4) - HK73*P(2,5);
+const float HK80 = HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK71*P(6,6) + HK72*P(4,6) - HK73*P(5,6);
+const float HK81 = 1.0F/(HK29*HK77 + HK30*HK79 + HK32*HK74 - HK34*HK78 + HK71*HK80 + HK72*HK75 - HK73*HK76 + R_VEL);
 
 
 // Observation Jacobians - axis 0
-Hfusion.at<0>() = 2*HK0 + 2*HK2;
-Hfusion.at<1>() = 2*HK5;
-Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9;
-Hfusion.at<3>() = 2*HK13;
-Hfusion.at<4>() = HK15 + HK17;
-Hfusion.at<5>() = 2*HK20;
-Hfusion.at<6>() = 2*HK21 - 2*HK22;
+Hfusion.at<0>() = HK4;
+Hfusion.at<1>() = HK6;
+Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9;
+Hfusion.at<3>() = HK14;
+Hfusion.at<4>() = HK21;
+Hfusion.at<5>() = 2*HK24;
+Hfusion.at<6>() = 2*HK25 - 2*HK26;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -112,43 +108,45 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 0
-Kfusion(0) = -HK32*HK39;
-Kfusion(1) = -HK34*HK39;
-Kfusion(2) = -HK38*HK39;
-Kfusion(3) = -HK37*HK39;
-Kfusion(4) = -HK36*HK39;
-Kfusion(5) = -HK33*HK39;
-Kfusion(6) = -HK35*HK39;
-Kfusion(7) = -HK39*(-HK23*P(1,7) - HK24*P(5,7) + HK25*P(0,7) + HK26*P(6,7) - HK27*P(3,7) + HK29*P(4,7) + HK31*P(2,7));
-Kfusion(8) = -HK39*(-HK23*P(1,8) - HK24*P(5,8) + HK25*P(0,8) + HK26*P(6,8) - HK27*P(3,8) + HK29*P(4,8) + HK31*P(2,8));
-Kfusion(9) = -HK39*(-HK23*P(1,9) - HK24*P(5,9) + HK25*P(0,9) + HK26*P(6,9) - HK27*P(3,9) + HK29*P(4,9) + HK31*P(2,9));
-Kfusion(10) = -HK39*(-HK23*P(1,10) - HK24*P(5,10) + HK25*P(0,10) + HK26*P(6,10) - HK27*P(3,10) + HK29*P(4,10) + HK31*P(2,10));
-Kfusion(11) = -HK39*(-HK23*P(1,11) - HK24*P(5,11) + HK25*P(0,11) + HK26*P(6,11) - HK27*P(3,11) + HK29*P(4,11) + HK31*P(2,11));
-Kfusion(12) = -HK39*(-HK23*P(1,12) - HK24*P(5,12) + HK25*P(0,12) + HK26*P(6,12) - HK27*P(3,12) + HK29*P(4,12) + HK31*P(2,12));
-Kfusion(13) = -HK39*(-HK23*P(1,13) - HK24*P(5,13) + HK25*P(0,13) + HK26*P(6,13) - HK27*P(3,13) + HK29*P(4,13) + HK31*P(2,13));
-Kfusion(14) = -HK39*(-HK23*P(1,14) - HK24*P(5,14) + HK25*P(0,14) + HK26*P(6,14) - HK27*P(3,14) + HK29*P(4,14) + HK31*P(2,14));
-Kfusion(15) = -HK39*(-HK23*P(1,15) - HK24*P(5,15) + HK25*P(0,15) + HK26*P(6,15) - HK27*P(3,15) + HK29*P(4,15) + HK31*P(2,15));
-Kfusion(16) = -HK39*(-HK23*P(1,16) - HK24*P(5,16) + HK25*P(0,16) + HK26*P(6,16) - HK27*P(3,16) + HK29*P(4,16) + HK31*P(2,16));
-Kfusion(17) = -HK39*(-HK23*P(1,17) - HK24*P(5,17) + HK25*P(0,17) + HK26*P(6,17) - HK27*P(3,17) + HK29*P(4,17) + HK31*P(2,17));
-Kfusion(18) = -HK39*(-HK23*P(1,18) - HK24*P(5,18) + HK25*P(0,18) + HK26*P(6,18) - HK27*P(3,18) + HK29*P(4,18) + HK31*P(2,18));
-Kfusion(19) = -HK39*(-HK23*P(1,19) - HK24*P(5,19) + HK25*P(0,19) + HK26*P(6,19) - HK27*P(3,19) + HK29*P(4,19) + HK31*P(2,19));
-Kfusion(20) = -HK39*(-HK23*P(1,20) - HK24*P(5,20) + HK25*P(0,20) + HK26*P(6,20) - HK27*P(3,20) + HK29*P(4,20) + HK31*P(2,20));
-Kfusion(21) = -HK39*(-HK23*P(1,21) - HK24*P(5,21) + HK25*P(0,21) + HK26*P(6,21) - HK27*P(3,21) + HK29*P(4,21) + HK31*P(2,21));
-Kfusion(22) = -HK39*(-HK23*P(1,22) - HK24*P(5,22) + HK25*P(0,22) + HK26*P(6,22) - HK27*P(3,22) + HK29*P(4,22) + HK31*P(2,22));
-Kfusion(23) = -HK39*(-HK23*P(1,23) - HK24*P(5,23) + HK25*P(0,23) + HK26*P(6,23) - HK27*P(3,23) + HK29*P(4,23) + HK31*P(2,23));
+Kfusion(0) = HK36*HK49;
+Kfusion(1) = HK41*HK49;
+Kfusion(2) = HK44*HK49;
+Kfusion(3) = HK47*HK49;
+Kfusion(4) = HK48*HK49;
+Kfusion(5) = HK37*HK49;
+Kfusion(6) = HK38*HK49;
+Kfusion(7) = HK49*(HK21*P(4,7) + HK27*P(5,7) - HK28*P(6,7) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7));
+Kfusion(8) = HK49*(HK21*P(4,8) + HK27*P(5,8) - HK28*P(6,8) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8));
+Kfusion(9) = HK49*(HK21*P(4,9) + HK27*P(5,9) - HK28*P(6,9) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9));
+Kfusion(10) = HK49*(HK21*P(4,10) + HK27*P(5,10) - HK28*P(6,10) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10));
+Kfusion(11) = HK49*(HK21*P(4,11) + HK27*P(5,11) - HK28*P(6,11) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11));
+Kfusion(12) = HK49*(HK21*P(4,12) + HK27*P(5,12) - HK28*P(6,12) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12));
+Kfusion(13) = HK49*(HK21*P(4,13) + HK27*P(5,13) - HK28*P(6,13) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13));
+Kfusion(14) = HK49*(HK21*P(4,14) + HK27*P(5,14) - HK28*P(6,14) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14));
+Kfusion(15) = HK49*(HK21*P(4,15) + HK27*P(5,15) - HK28*P(6,15) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15));
+Kfusion(16) = HK49*(HK21*P(4,16) + HK27*P(5,16) - HK28*P(6,16) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16));
+Kfusion(17) = HK49*(HK21*P(4,17) + HK27*P(5,17) - HK28*P(6,17) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17));
+Kfusion(18) = HK49*(HK21*P(4,18) + HK27*P(5,18) - HK28*P(6,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18));
+Kfusion(19) = HK49*(HK21*P(4,19) + HK27*P(5,19) - HK28*P(6,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19));
+Kfusion(20) = HK49*(HK21*P(4,20) + HK27*P(5,20) - HK28*P(6,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20));
+Kfusion(21) = HK49*(HK21*P(4,21) + HK27*P(5,21) - HK28*P(6,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21));
+Kfusion(22) = HK49*(HK21*P(4,22) + HK27*P(5,22) - HK28*P(6,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22));
+Kfusion(23) = HK49*(HK21*P(4,23) + HK27*P(5,23) - HK28*P(6,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23));
+Kfusion(24) = HK49*(HK21*P(4,24) + HK27*P(5,24) - HK28*P(6,24) + HK29*P(1,24) + HK30*P(0,24) - HK32*P(2,24) + HK34*P(3,24));
 
 
 // Observation Jacobians - axis 1
-Hfusion.at<0>() = 2*HK41;
-Hfusion.at<1>() = 2*HK42;
-Hfusion.at<2>() = 2*HK44;
-Hfusion.at<3>() = 2*HK1 - 2*HK45 - 2*HK46;
-Hfusion.at<4>() = -2*HK18 + 2*HK19;
-Hfusion.at<5>() = HK17 + HK48;
-Hfusion.at<6>() = 2*HK51;
+Hfusion.at<0>() = HK14;
+Hfusion.at<1>() = HK50;
+Hfusion.at<2>() = HK6;
+Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2;
+Hfusion.at<4>() = -2*HK22 + 2*HK23;
+Hfusion.at<5>() = HK52;
+Hfusion.at<6>() = 2*HK55;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -166,43 +164,45 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 1
-Kfusion(0) = HK59*HK66;
-Kfusion(1) = HK63*HK66;
-Kfusion(2) = HK61*HK66;
-Kfusion(3) = HK65*HK66;
-Kfusion(4) = HK62*HK66;
-Kfusion(5) = HK64*HK66;
-Kfusion(6) = HK60*HK66;
-Kfusion(7) = HK66*(HK52*P(2,7) + HK53*P(6,7) + HK54*P(0,7) - HK55*P(4,7) + HK56*P(1,7) - HK57*P(3,7) - HK58*P(5,7));
-Kfusion(8) = HK66*(HK52*P(2,8) + HK53*P(6,8) + HK54*P(0,8) - HK55*P(4,8) + HK56*P(1,8) - HK57*P(3,8) - HK58*P(5,8));
-Kfusion(9) = HK66*(HK52*P(2,9) + HK53*P(6,9) + HK54*P(0,9) - HK55*P(4,9) + HK56*P(1,9) - HK57*P(3,9) - HK58*P(5,9));
-Kfusion(10) = HK66*(HK52*P(2,10) + HK53*P(6,10) + HK54*P(0,10) - HK55*P(4,10) + HK56*P(1,10) - HK57*P(3,10) - HK58*P(5,10));
-Kfusion(11) = HK66*(HK52*P(2,11) + HK53*P(6,11) + HK54*P(0,11) - HK55*P(4,11) + HK56*P(1,11) - HK57*P(3,11) - HK58*P(5,11));
-Kfusion(12) = HK66*(HK52*P(2,12) + HK53*P(6,12) + HK54*P(0,12) - HK55*P(4,12) + HK56*P(1,12) - HK57*P(3,12) - HK58*P(5,12));
-Kfusion(13) = HK66*(HK52*P(2,13) + HK53*P(6,13) + HK54*P(0,13) - HK55*P(4,13) + HK56*P(1,13) - HK57*P(3,13) - HK58*P(5,13));
-Kfusion(14) = HK66*(HK52*P(2,14) + HK53*P(6,14) + HK54*P(0,14) - HK55*P(4,14) + HK56*P(1,14) - HK57*P(3,14) - HK58*P(5,14));
-Kfusion(15) = HK66*(HK52*P(2,15) + HK53*P(6,15) + HK54*P(0,15) - HK55*P(4,15) + HK56*P(1,15) - HK57*P(3,15) - HK58*P(5,15));
-Kfusion(16) = HK66*(HK52*P(2,16) + HK53*P(6,16) + HK54*P(0,16) - HK55*P(4,16) + HK56*P(1,16) - HK57*P(3,16) - HK58*P(5,16));
-Kfusion(17) = HK66*(HK52*P(2,17) + HK53*P(6,17) + HK54*P(0,17) - HK55*P(4,17) + HK56*P(1,17) - HK57*P(3,17) - HK58*P(5,17));
-Kfusion(18) = HK66*(HK52*P(2,18) + HK53*P(6,18) + HK54*P(0,18) - HK55*P(4,18) + HK56*P(1,18) - HK57*P(3,18) - HK58*P(5,18));
-Kfusion(19) = HK66*(HK52*P(2,19) + HK53*P(6,19) + HK54*P(0,19) - HK55*P(4,19) + HK56*P(1,19) - HK57*P(3,19) - HK58*P(5,19));
-Kfusion(20) = HK66*(HK52*P(2,20) + HK53*P(6,20) + HK54*P(0,20) - HK55*P(4,20) + HK56*P(1,20) - HK57*P(3,20) - HK58*P(5,20));
-Kfusion(21) = HK66*(HK52*P(2,21) + HK53*P(6,21) + HK54*P(0,21) - HK55*P(4,21) + HK56*P(1,21) - HK57*P(3,21) - HK58*P(5,21));
-Kfusion(22) = HK66*(HK52*P(2,22) + HK53*P(6,22) + HK54*P(0,22) - HK55*P(4,22) + HK56*P(1,22) - HK57*P(3,22) - HK58*P(5,22));
-Kfusion(23) = HK66*(HK52*P(2,23) + HK53*P(6,23) + HK54*P(0,23) - HK55*P(4,23) + HK56*P(1,23) - HK57*P(3,23) - HK58*P(5,23));
+Kfusion(0) = HK59*HK69;
+Kfusion(1) = HK65*HK69;
+Kfusion(2) = HK63*HK69;
+Kfusion(3) = HK67*HK69;
+Kfusion(4) = HK61*HK69;
+Kfusion(5) = HK68*HK69;
+Kfusion(6) = HK60*HK69;
+Kfusion(7) = HK69*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK52*P(5,7) + HK56*P(6,7) - HK57*P(4,7));
+Kfusion(8) = HK69*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK52*P(5,8) + HK56*P(6,8) - HK57*P(4,8));
+Kfusion(9) = HK69*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK52*P(5,9) + HK56*P(6,9) - HK57*P(4,9));
+Kfusion(10) = HK69*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK52*P(5,10) + HK56*P(6,10) - HK57*P(4,10));
+Kfusion(11) = HK69*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK52*P(5,11) + HK56*P(6,11) - HK57*P(4,11));
+Kfusion(12) = HK69*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK52*P(5,12) + HK56*P(6,12) - HK57*P(4,12));
+Kfusion(13) = HK69*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK52*P(5,13) + HK56*P(6,13) - HK57*P(4,13));
+Kfusion(14) = HK69*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK52*P(5,14) + HK56*P(6,14) - HK57*P(4,14));
+Kfusion(15) = HK69*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK52*P(5,15) + HK56*P(6,15) - HK57*P(4,15));
+Kfusion(16) = HK69*(HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK52*P(5,16) + HK56*P(6,16) - HK57*P(4,16));
+Kfusion(17) = HK69*(HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK52*P(5,17) + HK56*P(6,17) - HK57*P(4,17));
+Kfusion(18) = HK69*(HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK52*P(5,18) + HK56*P(6,18) - HK57*P(4,18));
+Kfusion(19) = HK69*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK52*P(5,19) + HK56*P(6,19) - HK57*P(4,19));
+Kfusion(20) = HK69*(HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK52*P(5,20) + HK56*P(6,20) - HK57*P(4,20));
+Kfusion(21) = HK69*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK52*P(5,21) + HK56*P(6,21) - HK57*P(4,21));
+Kfusion(22) = HK69*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK52*P(5,22) + HK56*P(6,22) - HK57*P(4,22));
+Kfusion(23) = HK69*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK52*P(5,23) + HK56*P(6,23) - HK57*P(4,23));
+Kfusion(24) = HK69*(HK29*P(2,24) - HK30*P(3,24) + HK32*P(1,24) + HK34*P(0,24) + HK52*P(5,24) + HK56*P(6,24) - HK57*P(4,24));
 
 
 // Observation Jacobians - axis 2
-Hfusion.at<0>() = 2*HK30 + 2*HK8;
-Hfusion.at<1>() = -2*HK10 + 2*HK12 - 2*HK67;
-Hfusion.at<2>() = 2*HK68;
-Hfusion.at<3>() = 2*HK69;
+Hfusion.at<0>() = HK50;
+Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12;
+Hfusion.at<2>() = HK4;
+Hfusion.at<3>() = HK6;
 Hfusion.at<4>() = 2*HK70;
-Hfusion.at<5>() = -2*HK49 + 2*HK50;
-Hfusion.at<6>() = HK15 + HK48 + 1;
+Hfusion.at<5>() = -2*HK53 + 2*HK54;
+Hfusion.at<6>() = HK71;
 Hfusion.at<7>() = 0;
 Hfusion.at<8>() = 0;
 Hfusion.at<9>() = 0;
@@ -220,30 +220,32 @@ Hfusion.at<20>() = 0;
 Hfusion.at<21>() = 0;
 Hfusion.at<22>() = 0;
 Hfusion.at<23>() = 0;
+Hfusion.at<24>() = 0;
 
 
 // Kalman gains - axis 2
-Kfusion(0) = -HK78*HK85;
-Kfusion(1) = -HK84*HK85;
-Kfusion(2) = -HK83*HK85;
-Kfusion(3) = -HK80*HK85;
-Kfusion(4) = -HK79*HK85;
-Kfusion(5) = -HK81*HK85;
-Kfusion(6) = -HK82*HK85;
-Kfusion(7) = -HK85*(-HK71*P(3,7) - HK72*P(4,7) + HK73*P(0,7) + HK74*P(5,7) - HK75*P(2,7) + HK76*P(6,7) + HK77*P(1,7));
-Kfusion(8) = -HK85*(-HK71*P(3,8) - HK72*P(4,8) + HK73*P(0,8) + HK74*P(5,8) - HK75*P(2,8) + HK76*P(6,8) + HK77*P(1,8));
-Kfusion(9) = -HK85*(-HK71*P(3,9) - HK72*P(4,9) + HK73*P(0,9) + HK74*P(5,9) - HK75*P(2,9) + HK76*P(6,9) + HK77*P(1,9));
-Kfusion(10) = -HK85*(-HK71*P(3,10) - HK72*P(4,10) + HK73*P(0,10) + HK74*P(5,10) - HK75*P(2,10) + HK76*P(6,10) + HK77*P(1,10));
-Kfusion(11) = -HK85*(-HK71*P(3,11) - HK72*P(4,11) + HK73*P(0,11) + HK74*P(5,11) - HK75*P(2,11) + HK76*P(6,11) + HK77*P(1,11));
-Kfusion(12) = -HK85*(-HK71*P(3,12) - HK72*P(4,12) + HK73*P(0,12) + HK74*P(5,12) - HK75*P(2,12) + HK76*P(6,12) + HK77*P(1,12));
-Kfusion(13) = -HK85*(-HK71*P(3,13) - HK72*P(4,13) + HK73*P(0,13) + HK74*P(5,13) - HK75*P(2,13) + HK76*P(6,13) + HK77*P(1,13));
-Kfusion(14) = -HK85*(-HK71*P(3,14) - HK72*P(4,14) + HK73*P(0,14) + HK74*P(5,14) - HK75*P(2,14) + HK76*P(6,14) + HK77*P(1,14));
-Kfusion(15) = -HK85*(-HK71*P(3,15) - HK72*P(4,15) + HK73*P(0,15) + HK74*P(5,15) - HK75*P(2,15) + HK76*P(6,15) + HK77*P(1,15));
-Kfusion(16) = -HK85*(-HK71*P(3,16) - HK72*P(4,16) + HK73*P(0,16) + HK74*P(5,16) - HK75*P(2,16) + HK76*P(6,16) + HK77*P(1,16));
-Kfusion(17) = -HK85*(-HK71*P(3,17) - HK72*P(4,17) + HK73*P(0,17) + HK74*P(5,17) - HK75*P(2,17) + HK76*P(6,17) + HK77*P(1,17));
-Kfusion(18) = -HK85*(-HK71*P(3,18) - HK72*P(4,18) + HK73*P(0,18) + HK74*P(5,18) - HK75*P(2,18) + HK76*P(6,18) + HK77*P(1,18));
-Kfusion(19) = -HK85*(-HK71*P(3,19) - HK72*P(4,19) + HK73*P(0,19) + HK74*P(5,19) - HK75*P(2,19) + HK76*P(6,19) + HK77*P(1,19));
-Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20) - HK75*P(2,20) + HK76*P(6,20) + HK77*P(1,20));
-Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21));
-Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22));
-Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23));
+Kfusion(0) = HK74*HK81;
+Kfusion(1) = HK78*HK81;
+Kfusion(2) = HK79*HK81;
+Kfusion(3) = HK77*HK81;
+Kfusion(4) = HK75*HK81;
+Kfusion(5) = HK76*HK81;
+Kfusion(6) = HK80*HK81;
+Kfusion(7) = HK81*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK71*P(6,7) + HK72*P(4,7) - HK73*P(5,7));
+Kfusion(8) = HK81*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK71*P(6,8) + HK72*P(4,8) - HK73*P(5,8));
+Kfusion(9) = HK81*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK71*P(6,9) + HK72*P(4,9) - HK73*P(5,9));
+Kfusion(10) = HK81*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK71*P(6,10) + HK72*P(4,10) - HK73*P(5,10));
+Kfusion(11) = HK81*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK71*P(6,11) + HK72*P(4,11) - HK73*P(5,11));
+Kfusion(12) = HK81*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK71*P(6,12) + HK72*P(4,12) - HK73*P(5,12));
+Kfusion(13) = HK81*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK71*P(6,13) + HK72*P(4,13) - HK73*P(5,13));
+Kfusion(14) = HK81*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK71*P(6,14) + HK72*P(4,14) - HK73*P(5,14));
+Kfusion(15) = HK81*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK71*P(6,15) + HK72*P(4,15) - HK73*P(5,15));
+Kfusion(16) = HK81*(HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK71*P(6,16) + HK72*P(4,16) - HK73*P(5,16));
+Kfusion(17) = HK81*(HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK71*P(6,17) + HK72*P(4,17) - HK73*P(5,17));
+Kfusion(18) = HK81*(HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK71*P(6,18) + HK72*P(4,18) - HK73*P(5,18));
+Kfusion(19) = HK81*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK71*P(6,19) + HK72*P(4,19) - HK73*P(5,19));
+Kfusion(20) = HK81*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK71*P(6,20) + HK72*P(4,20) - HK73*P(5,20));
+Kfusion(21) = HK81*(HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK71*P(6,21) + HK72*P(4,21) - HK73*P(5,21));
+Kfusion(22) = HK81*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK71*P(6,22) + HK72*P(4,22) - HK73*P(5,22));
+Kfusion(23) = HK81*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK71*P(6,23) + HK72*P(4,23) - HK73*P(5,23));
+Kfusion(24) = HK81*(HK29*P(3,24) + HK30*P(2,24) + HK32*P(0,24) - HK34*P(1,24) + HK71*P(6,24) + HK72*P(4,24) - HK73*P(5,24));

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp

@@ -1,66 +1,63 @@
 // Intermediate variables
 const float t0 = powf(P(0,1), 2);
-const float t1 = -t0;
-const float t2 = P(0,0)*P(1,1) + P(0,0)*velObsVar + P(1,1)*velObsVar + t1 + powf(velObsVar, 2);
-const float t3 = 1.0F/t2;
-const float t4 = P(1,1) + velObsVar;
-const float t5 = P(0,1)*t3;
-const float t6 = -t5;
-const float t7 = P(0,0) + velObsVar;
-const float t8 = P(0,0)*t4 + t1;
-const float t9 = t5*velObsVar;
-const float t10 = P(1,1)*t7;
-const float t11 = t1 + t10;
-const float t12 = P(0,1)*P(1,2);
-const float t13 = P(0,2)*t4;
-const float t14 = P(0,1)*P(0,2);
-const float t15 = P(1,2)*t7;
-const float t16 = t0*velObsVar;
-const float t17 = powf(t2, -2);
-const float t18 = t4*velObsVar + t8;
-const float t19 = t17*t18;
-const float t20 = t17*(t16 + t7*t8);
-const float t21 = t0 - t10;
-const float t22 = t17*t21;
-const float t23 = t14 - t15;
-const float t24 = P(0,1)*t23;
-const float t25 = t12 - t13;
-const float t26 = t16 - t21*t4;
-const float t27 = t17*t26;
-const float t28 = t11 + t7*velObsVar;
-const float t29 = t17*t8;
-const float t30 = t17*t28;
-const float t31 = P(0,1)*t25;
-const float t32 = t23*t4 + t31;
-const float t33 = t17*t32;
-const float t34 = P(0,1)*velObsVar;
-const float t35 = t24 + t25*t7;
-const float t36 = t17*t35;
+const float t1 = P(0,0) + velObsVar;
+const float t2 = P(1,1) + velObsVar;
+const float t3 = t1*t2;
+const float t4 = 1.0F/(t0 - t3);
+const float t5 = P(0,1)*t4;
+const float t6 = t1*t4;
+const float t7 = -P(0,0)*t2 + t0;
+const float t8 = t4*t7;
+const float t9 = -t5*velObsVar;
+const float t10 = P(1,1)*t1;
+const float t11 = t0 - t10;
+const float t12 = P(0,1)*P(1,2) - P(0,2)*t2;
+const float t13 = P(0,1)*P(0,2) - P(1,2)*t1;
+const float t14 = -t0;
+const float t15 = t14 + t3;
+const float t16 = 1.0F/t15;
+const float t17 = t16*velObsVar;
+const float t18 = t17*t2 + t8;
+const float t19 = t0*velObsVar;
+const float t20 = t16*t19;
+const float t21 = t20 + t6*t7;
+const float t22 = t16*t18;
+const float t23 = P(0,1)*t13;
+const float t24 = powf(t15, -2);
+const float t25 = t24*(-t11*t2 + t19);
+const float t26 = t1*velObsVar + t10 + t14;
+const float t27 = t16*t8;
+const float t28 = t24*t26;
+const float t29 = P(0,1)*t12;
+const float t30 = t24*(t13*t2 + t29);
+const float t31 = P(0,1)*velObsVar;
+const float t32 = t1*t12 + t23;
+const float t33 = t24*t32;
 
 
 // Equations for NE velocity innovation variance's determinante inverse
-_ekf_gsf[model_index].S_det_inverse = t3;
+_ekf_gsf[model_index].S_det_inverse = -t4;
 
 
 // Equations for NE velocity innovation variance inverse
-_ekf_gsf[model_index].S_inverse(0,0) = t3*t4;
-_ekf_gsf[model_index].S_inverse(0,1) = t6;
-_ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
+_ekf_gsf[model_index].S_inverse(0,0) = -t2*t4;
+_ekf_gsf[model_index].S_inverse(0,1) = t5;
+_ekf_gsf[model_index].S_inverse(1,1) = -t6;
 
 
 // Equations for NE velocity Kalman gain
-K(0,0) = t3*t8;
+K(0,0) = t8;
 K(1,0) = t9;
-K(2,0) = t3*(-t12 + t13);
+K(2,0) = t12*t4;
 K(0,1) = t9;
-K(1,1) = t11*t3;
-K(2,1) = t3*(-t14 + t15);
+K(1,1) = t11*t4;
+K(2,1) = t13*t4;
 
 
 // Equations for covariance matrix update
-_ekf_gsf[model_index].P(0,0) = P(0,0) - t16*t19 - t20*t8;
-_ekf_gsf[model_index].P(0,1) = P(0,1)*(t18*t22 - t20*velObsVar + 1);
-_ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26;
-_ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25;
-_ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31;
-_ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36;
+_ekf_gsf[model_index].P(0,0) = P(0,0) - t18*t20 - t21*t8;
+_ekf_gsf[model_index].P(0,1) = P(0,1)*(t11*t22 - t17*t21 + 1);
+_ekf_gsf[model_index].P(1,1) = P(1,1) + t11*t25 - t19*t28;
+_ekf_gsf[model_index].P(0,2) = P(0,2) + t12*t16*t21 + t22*t23;
+_ekf_gsf[model_index].P(1,2) = P(1,2) + t13*t25 + t28*t29;
+_ekf_gsf[model_index].P(2,2) = P(2,2) - t12*t33 - t13*t30;

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp

@@ -3,8 +3,8 @@
 #include <cstdlib>
 #include "../../../../../matrix/matrix/math.hpp"
 
-typedef matrix::Vector<float, 24> Vector24f;
-typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+typedef matrix::Vector<float, 24> Vector25f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
 
 float sq(float in) {
     return in * in;

src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp

@@ -1,19 +1,20 @@
 // calculate 321 yaw observation matrix - option A
-const float SA0 = 2*q0;
-const float SA1 = 2*q1;
-const float SA2 = SA0*q3 + SA1*q2;
-const float SA3 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
+const float SA0 = 2*q3;
+const float SA1 = 2*q2;
+const float SA2 = SA0*q0 + SA1*q1;
+const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
 const float SA4 = powf(SA3, -2);
 const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
 const float SA6 = 1.0F/SA3;
-const float SA7 = 2*SA5*SA6;
-const float SA8 = 4*SA2*SA4;
+const float SA7 = SA2*SA4;
+const float SA8 = 2*SA7;
+const float SA9 = 2*SA6;
 
 
-H_YAW.at<0>() = SA7*q3;
-H_YAW.at<1>() = SA7*q2;
-H_YAW.at<2>() = SA5*(SA1*SA6 + SA8*q2);
-H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3);
+H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0);
+H_YAW.at<1>() = SA5*(SA1*SA6 - SA8*q1);
+H_YAW.at<2>() = SA5*(SA1*SA7 + SA9*q1);
+H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
 H_YAW.at<4>() = 0;
 H_YAW.at<5>() = 0;
 H_YAW.at<6>() = 0;
@@ -34,6 +35,7 @@ H_YAW.at<20>() = 0;
 H_YAW.at<21>() = 0;
 H_YAW.at<22>() = 0;
 H_YAW.at<23>() = 0;
+H_YAW.at<24>() = 0;
 
 
 // calculate 321 yaw observation matrix - option B
@@ -41,17 +43,18 @@ const float SB0 = 2*q0;
 const float SB1 = 2*q1;
 const float SB2 = SB0*q3 + SB1*q2;
 const float SB3 = powf(SB2, -2);
-const float SB4 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1;
+const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
 const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
-const float SB6 = SB3*SB4;
-const float SB7 = 2*SB5*SB6;
-const float SB8 = 4/SB2;
+const float SB6 = 1.0F/SB2;
+const float SB7 = SB3*SB4;
+const float SB8 = 2*SB7;
+const float SB9 = 2*SB6;
 
 
-H_YAW.at<0>() = SB7*q3;
-H_YAW.at<1>() = SB7*q2;
-H_YAW.at<2>() = -SB5*(-SB1*SB6 - SB8*q2);
-H_YAW.at<3>() = -SB5*(-SB0*SB6 - SB8*q3);
+H_YAW.at<0>() = -SB5*(SB0*SB6 - SB8*q3);
+H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2);
+H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2);
+H_YAW.at<3>() = -SB5*(-SB0*SB7 - SB9*q3);
 H_YAW.at<4>() = 0;
 H_YAW.at<5>() = 0;
 H_YAW.at<6>() = 0;
@@ -72,24 +75,26 @@ H_YAW.at<20>() = 0;
 H_YAW.at<21>() = 0;
 H_YAW.at<22>() = 0;
 H_YAW.at<23>() = 0;
+H_YAW.at<24>() = 0;
 
 
 // calculate 312 yaw observation matrix - option A
-const float SA0 = 2*q0;
+const float SA0 = 2*q3;
 const float SA1 = 2*q2;
-const float SA2 = SA0*q3 - SA1*q1;
-const float SA3 = -2*powf(q1, 2) - 2*powf(q3, 2) + 1;
+const float SA2 = SA0*q0 - SA1*q1;
+const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
 const float SA4 = powf(SA3, -2);
 const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
 const float SA6 = 1.0F/SA3;
-const float SA7 = 2*SA5*SA6;
-const float SA8 = 4*SA2*SA4;
+const float SA7 = SA2*SA4;
+const float SA8 = 2*SA7;
+const float SA9 = 2*SA6;
 
 
-H_YAW.at<0>() = SA7*q3;
+H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0);
 H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1);
-H_YAW.at<2>() = -SA7*q1;
-H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3);
+H_YAW.at<2>() = SA5*(-SA1*SA7 - SA9*q1);
+H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
 H_YAW.at<4>() = 0;
 H_YAW.at<5>() = 0;
 H_YAW.at<6>() = 0;
@@ -110,24 +115,26 @@ H_YAW.at<20>() = 0;
 H_YAW.at<21>() = 0;
 H_YAW.at<22>() = 0;
 H_YAW.at<23>() = 0;
+H_YAW.at<24>() = 0;
 
 
 // calculate 312 yaw observation matrix - option B
 const float SB0 = 2*q0;
-const float SB1 = 2*q2;
-const float SB2 = -SB0*q3 + SB1*q1;
+const float SB1 = 2*q1;
+const float SB2 = -SB0*q3 + SB1*q2;
 const float SB3 = powf(SB2, -2);
-const float SB4 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
+const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
 const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
-const float SB6 = SB3*SB4;
-const float SB7 = 2*SB5*SB6;
-const float SB8 = 4/SB2;
+const float SB6 = 1.0F/SB2;
+const float SB7 = SB3*SB4;
+const float SB8 = 2*SB7;
+const float SB9 = 2*SB6;
 
 
-H_YAW.at<0>() = -SB7*q3;
-H_YAW.at<1>() = -SB5*(-SB1*SB6 + SB8*q1);
-H_YAW.at<2>() = SB7*q1;
-H_YAW.at<3>() = -SB5*(SB0*SB6 + SB8*q3);
+H_YAW.at<0>() = -SB5*(-SB0*SB6 + SB8*q3);
+H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2);
+H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2);
+H_YAW.at<3>() = -SB5*(SB0*SB7 + SB9*q3);
 H_YAW.at<4>() = 0;
 H_YAW.at<5>() = 0;
 H_YAW.at<6>() = 0;
@@ -148,3 +155,4 @@ H_YAW.at<20>() = 0;
 H_YAW.at<21>() = 0;
 H_YAW.at<22>() = 0;
 H_YAW.at<23>() = 0;
+H_YAW.at<24>() = 0;

src/modules/ekf2/EKF/python/ekf_derivation/main.py

@@ -6,22 +6,23 @@ from code_gen import *
 # q: quaternion describing rotation from frame 1 to frame 2
 # returns a rotation matrix derived form q which describes the same
 # rotation
-def quat2Rot(q):
+def quat2Rot(q, use_legacy_method=0):
     q0 = q[0]
     q1 = q[1]
     q2 = q[2]
     q3 = q[3]
 
-    # Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
-    #               [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
-    #                [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
-
-    # Use the simplified formula for unit quaternion to rotation matrix
-    # as it produces a simpler and more stable EKF derivation given
-    # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
-    Rot = Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
-                  [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
-                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])
+    if use_legacy_method == 1:
+        Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
+                    [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
+                    [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
+    else:
+        # Use the simplified formula for unit quaternion to rotation matrix
+        # as it produces a simpler and more stable EKF derivation given
+        # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
+        Rot = Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
+                    [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
+                    [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])
 
     return Rot
 
@@ -59,10 +60,10 @@ def quat_mult(p,q):
 
 def create_symmetric_cov_matrix():
     # define a symbolic covariance matrix
-    P = Matrix(24,24,create_cov_matrix)
+    P = Matrix(25,25,create_cov_matrix)
 
-    for index in range(24):
-        for j in range(24):
+    for index in range(25):
+        for j in range(25):
             if index > j:
                 P[index,j] = P[j,index]
 
@@ -96,16 +97,16 @@ def generate_observation_equations(P,state,observation,variance,varname="HK"):
 # generate equations for observation vector Jacobian and Kalman gain
 # n_obs is the vector dimension and must be >= 2
 def generate_observation_vector_equations(P,state,observation,variance,n_obs):
-    K = zeros(24,n_obs)
+    K = zeros(25,n_obs)
     H = observation.jacobian(state)
-    HK = zeros(n_obs*48,1)
+    HK = zeros(n_obs*50,1)
     for index in range(n_obs):
         H[index,:] = Matrix([observation[index]]).jacobian(state)
         innov_var = H[index,:] * P * H[index,:].T + Matrix([variance])
         assert(innov_var.shape[0] == 1)
         assert(innov_var.shape[1] == 1)
         K[:,index] = P * H[index,:].T / innov_var[0,0]
-        HK[index*48:(index+1)*48,0] = Matrix([H[index,:].transpose(), K[:,index]])
+        HK[index*50:(index+1)*50,0] = Matrix([H[index,:].transpose(), K[:,index]])
 
     HK_simple = cse(HK, symbols("HK0:1000"), optimizations='basic')
 
@@ -120,18 +121,29 @@ def write_equations_to_file(equations,code_generator_id,n_obs):
         code_generator_id.print_string("Sub Expressions")
         code_generator_id.write_subexpressions(equations[0])
         code_generator_id.print_string("Observation Jacobians")
-        code_generator_id.write_matrix(Matrix(equations[1][0][0:24]), "Hfusion", False, ".at<", ">()")
+        code_generator_id.write_matrix(Matrix(equations[1][0][0:25]), "Hfusion", False, ".at<", ">()")
         code_generator_id.print_string("Kalman gains")
-        code_generator_id.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
+        code_generator_id.write_matrix(Matrix(equations[1][0][25:]), "Kfusion", False, "(", ")")
     else:
         code_generator_id.print_string("Sub Expressions")
         code_generator_id.write_subexpressions(equations[0])
         for axis_index in range(n_obs):
-            start_index = axis_index*48
+            start_index = axis_index*50
             code_generator_id.print_string("Observation Jacobians - axis %i" % axis_index)
-            code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+24]), "Hfusion", False, ".at<", ">()")
+            code_generator_id.write_matrix(Matrix(equations[1][0][start_index:start_index+25]), "Hfusion", False, ".at<", ">()")
             code_generator_id.print_string("Kalman gains - axis %i" % axis_index)
-            code_generator_id.write_matrix(Matrix(equations[1][0][start_index+24:start_index+48]), "Kfusion", False, "(", ")")
+            code_generator_id.write_matrix(Matrix(equations[1][0][start_index+25:start_index+50]), "Kfusion", False, "(", ")")
+
+    return
+
+# derive equations for sequential fusion of optical flow measurements
+def hagl_observation(P,state):
+    obs_var = symbols("R_HAGL", real=True) # optical flow line of sight rate measurement noise variance
+    observation = state[24] - state[9]
+    equations = generate_observation_equations(P,state,observation,obs_var)
+    hagl_code_generator = CodeGenerator("./generated/hagl_generated.cpp")
+    write_equations_to_file(equations,hagl_code_generator,1)
+    hagl_code_generator.close()
 
     return
 
@@ -142,10 +154,16 @@ def optical_flow_observation(P,state,R_to_body,vx,vy,vz):
     obs_var = symbols("R_LOS", real=True) # optical flow line of sight rate measurement noise variance
 
     # Define rotation matrix from body to sensor frame
-    Tbs = Matrix(3,3,create_Tbs_matrix)
+    Tsb = Matrix(3,3,create_Tbs_matrix)
+
+    # define rotation from nav to sensor frame
+    Tsn = Tsb * R_to_body
+
+    hagl = state[24] - state[9]
+    range = hagl / Tsn[2,2]
 
     # Calculate earth relative velocity in a non-rotating sensor frame
-    relVelSensor = Tbs * R_to_body * Matrix([vx,vy,vz])
+    relVelSensor = Tsn * Matrix([vx,vy,vz])
 
     # Divide by range to get predicted angular LOS rates relative to X and Y
     # axes. Note these are rates in a non-rotating sensor frame
@@ -185,14 +203,14 @@ def body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz):
     vel_bf_code_generator = CodeGenerator("./generated/vel_bf_generated.cpp")
     axes = [0,1,2]
     H_obs = vel_bf.jacobian(state) # observation Jacobians
-    K_gain = zeros(24,3)
+    K_gain = zeros(25,3)
     for index in axes:
         equations = generate_observation_equations(P,state,vel_bf[index],obs_var)
 
         vel_bf_code_generator.print_string("axis %i" % index)
         vel_bf_code_generator.write_subexpressions(equations[0])
-        vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:24]), "H_VEL", False, "(", ")")
-        vel_bf_code_generator.write_matrix(Matrix(equations[1][0][24:]), "Kfusion", False, "(", ")")
+        vel_bf_code_generator.write_matrix(Matrix(equations[1][0][0:25]), "H_VEL", False, "(", ")")
+        vel_bf_code_generator.write_matrix(Matrix(equations[1][0][25:]), "Kfusion", False, "(", ")")
 
     vel_bf_code_generator.close()
 
@@ -270,7 +288,7 @@ def body_frame_accel_observation(P,state,R_to_body,vx,vy,vz,wx,wy):
 
     acc_bf_code_generator  = CodeGenerator("./generated/acc_bf_generated.cpp")
     H = observation.jacobian(state)
-    K = zeros(24,2)
+    K = zeros(25,2)
     axes = [0,1]
     for index in axes:
         equations = generate_observation_equations(P,state,observation[index],obs_var)
@@ -569,8 +587,11 @@ def generate_code():
     wx, wy = symbols("vwn, vwe", real=True)
     w = Matrix([wx,wy])
 
+    # vertical position of terrain underneath vehicle
+    ptz = symbols("ptd", real=True)
+
     # state vector at arbitrary time t
-    state = Matrix([q,v,p,d_ang_b,d_vel_b,i,ib,w])
+    state = Matrix([q, v, p, d_ang_b, d_vel_b, i, ib, w, ptz])
 
     print('Defining state propagation ...')
     q_new = quat_mult(q, Matrix([1, 0.5 * d_ang_true[0],  0.5 * d_ang_true[1],  0.5 * d_ang_true[2]]))
@@ -582,9 +603,10 @@ def generate_code():
     i_new = i
     ib_new = ib
     w_new = w
+    ptz_new = ptz
 
     # predicted state vector at time t + dt
-    state_new = Matrix([q_new, v_new, p_new, d_ang_b_new, d_vel_b_new, i_new, ib_new, w_new])
+    state_new = Matrix([q_new, v_new, p_new, d_ang_b_new, d_vel_b_new, i_new, ib_new, w_new, ptz_new])
 
     print('Computing state propagation jacobian ...')
     A = state_new.jacobian(state)
@@ -595,8 +617,8 @@ def generate_code():
     print('Computing covariance propagation ...')
     P_new = A * P * A.T + G * var_u * G.T
 
-    for index in range(24):
-        for j in range(24):
+    for index in range(25):
+        for j in range(25):
             if index > j:
                 P_new[index,j] = 0
 
@@ -611,6 +633,11 @@ def generate_code():
 
     cov_code_generator.close()
 
+    # use legacy quaternion to rotation matrix conversion for observaton equation as it gives
+    # simpler equations
+    R_to_earth = quat2Rot(q,1)
+    R_to_body = R_to_earth.T
+
     # derive autocode for observation methods
     print('Generating heading observation code ...')
     yaw_observation(P,state,R_to_earth)
@@ -627,6 +654,8 @@ def generate_code():
     beta_observation(P,state,R_to_body,vx,vy,vz,wx,wy)
     print('Generating optical flow observation code ...')
     optical_flow_observation(P,state,R_to_body,vx,vy,vz)
+    print('Generating HAGL observation code ...')
+    hagl_observation(P,state)
     print('Generating body frame velocity observation code ...')
     body_frame_velocity_observation(P,state,R_to_body,vx,vy,vz)
     print('Generating body frame acceleration observation code ...')

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -173,7 +173,7 @@ void Ekf::fuseSideslip()
 		}
 
 		// Observation Jacobians
-		SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
+		SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
 		Hfusion.at<0>() = HK21*HK22;
 		Hfusion.at<1>() = HK22*HK25;
 		Hfusion.at<2>() = HK22*HK26;
@@ -185,7 +185,7 @@ void Ekf::fuseSideslip()
 		Hfusion.at<23>() = HK16*HK32;
 
 		// Calculate Kalman gains
-		Vector24f Kfusion;
+		Vector25f Kfusion;
 
 		if (!update_wind_only) {
 

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -48,7 +48,7 @@ void Ekf::initHagl()
 	resetHaglFake();
 }
 
-void Ekf::runTerrainEstimator()
+void Ekf::controlHaglFusion()
 {
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
@@ -56,35 +56,17 @@ void Ekf::runTerrainEstimator()
 		_control_status.flags.rng_fault = false;
 	}
 
-	predictHagl();
-
 	controlHaglRngFusion();
-	controlHaglFlowFusion();
 	controlHaglFakeFusion();
 
-	// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
-	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
-		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
+	// constrain terrain position to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
+	if (_state.posd_terrain - _state.pos(2) < _params.rng_gnd_clearance) {
+		_state.posd_terrain = _params.rng_gnd_clearance + _state.pos(2);
 	}
 
 	updateTerrainValidity();
 }
 
-void Ekf::predictHagl()
-{
-	// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
-
-	// process noise due to errors in vehicle height estimate
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
-
-	// process noise due to terrain gradient
-	_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
-			* (sq(_state.vel(0)) + sq(_state.vel(1)));
-
-	// limit the variance to prevent it becoming badly conditioned
-	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
-}
-
 void Ekf::controlHaglRngFusion()
 {
 	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
@@ -103,7 +85,7 @@ void Ekf::controlHaglRngFusion()
 
 		if (_hagl_sensor_status.flags.range_finder) {
 			if (continuing_conditions_passing) {
-				fuseHaglRng();
+				/* fuseHaglRng(); */ // done when fusing range finder
 
 				// We have been rejecting range data for too long
 				const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
@@ -153,11 +135,11 @@ void Ekf::resetHaglRngIfNeeded()
 {
 	if (_hagl_sensor_status.flags.flow) {
 		const float meas_hagl = _range_sensor.getDistBottom();
-		const float pred_hagl = _terrain_vpos - _state.pos(2);
+		const float pred_hagl = _state.posd_terrain - _state.pos(2);
 		const float hagl_innov = pred_hagl - meas_hagl;
 		const float obs_variance = getRngVar();
 
-		const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
+		const float hagl_innov_var = fmaxf(P(24, 24) + obs_variance, obs_variance);
 
 		const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
 		const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var);
@@ -168,7 +150,7 @@ void Ekf::resetHaglRngIfNeeded()
 			resetHaglRng();
 
 		} else {
-			fuseHaglRng();
+			/* fuseHaglRng(); */
 		}
 
 	} else {
@@ -185,8 +167,8 @@ float Ekf::getRngVar()
 
 void Ekf::resetHaglRng()
 {
-	_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
-	_terrain_var = getRngVar();
+	_state.posd_terrain = _state.pos(2) + _range_sensor.getDistBottom();
+	P(24, 24) = getRngVar();
 	_terrain_vpos_reset_counter++;
 	_time_last_hagl_fuse = _time_last_imu;
 }
@@ -196,43 +178,6 @@ void Ekf::stopHaglRngFusion()
 	_hagl_sensor_status.flags.range_finder = false;
 }
 
-void Ekf::fuseHaglRng()
-{
-	// get a height above ground measurement from the range finder assuming a flat earth
-	const float meas_hagl = _range_sensor.getDistBottom();
-
-	// predict the hagl from the vehicle position and terrain height
-	const float pred_hagl = _terrain_vpos - _state.pos(2);
-
-	// calculate the innovation
-	_hagl_innov = pred_hagl - meas_hagl;
-
-	// calculate the observation variance adding the variance of the vehicles own height uncertainty
-	const float obs_variance = getRngVar();
-
-	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
-	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
-
-	// perform an innovation consistency check and only fuse data if it passes
-	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
-	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
-
-	if (_hagl_test_ratio <= 1.0f) {
-		// calculate the Kalman gain
-		const float gain = _terrain_var / _hagl_innov_var;
-		// correct the state
-		_terrain_vpos -= gain * _hagl_innov;
-		// correct the variance
-		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-		// record last successful fusion event
-		_time_last_hagl_fuse = _time_last_imu;
-		_innov_check_fail_status.flags.reject_hagl = false;
-
-	} else {
-		_innov_check_fail_status.flags.reject_hagl = true;
-	}
-}
-
 void Ekf::controlHaglFlowFusion()
 {
 	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
@@ -242,18 +187,15 @@ void Ekf::controlHaglFlowFusion()
 
 	if (_flow_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.in_air
-		                                           && !_control_status.flags.opt_flow
-							   && _control_status.flags.gps
-							   && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead?
-							   /* && !_hagl_sensor_status.flags.range_finder; */
+							   && _control_status.flags.gps;
 		const bool starting_conditions_passing = continuing_conditions_passing;
 
 		if (_hagl_sensor_status.flags.flow) {
 			if (continuing_conditions_passing) {
 
 				// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-				fuseFlowForTerrain();
-				_flow_data_ready = false;
+				/* fuseFlowForTerrain(); */
+				/* _flow_data_ready = false; */ // done in flow fusion
 
 				// TODO: do something when failing continuously the innovation check
 				/* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */
@@ -283,8 +225,8 @@ void Ekf::startHaglFlowFusion()
 {
 	_hagl_sensor_status.flags.flow = true;
 	// TODO: do a reset instead of trying to fuse the data?
-	fuseFlowForTerrain();
-	_flow_data_ready = false;
+	/* fuseFlowForTerrain(); */
+	/* _flow_data_ready = false; */
 }
 
 void Ekf::stopHaglFlowFusion()
@@ -295,111 +237,11 @@ void Ekf::stopHaglFlowFusion()
 void Ekf::resetHaglFlow()
 {
 	// TODO: use the flow data
-	_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
-	_terrain_var = 100.0f;
+	_state.posd_terrain = fmaxf(0.0f,  _state.pos(2));
+	P(24, 24) = 100.0f;
 	_terrain_vpos_reset_counter++;
 }
 
-void Ekf::fuseFlowForTerrain()
-{
-	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
-	// correct for gyro bias errors in the data used to do the motion compensation
-	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
-	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
-
-	// get latest estimated orientation
-	const float q0 = _state.quat_nominal(0);
-	const float q1 = _state.quat_nominal(1);
-	const float q2 = _state.quat_nominal(2);
-	const float q3 = _state.quat_nominal(3);
-
-	// calculate the optical flow observation variance
-	const float R_LOS = calcOptFlowMeasVar();
-
-	// get rotation matrix from earth to body
-	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
-
-	// calculate the sensor position relative to the IMU
-	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
-
-	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
-
-	// calculate the velocity of the sensor in the earth frame
-	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
-
-	// rotate into body frame
-	const Vector3f vel_body = earth_to_body * vel_rel_earth;
-
-	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
-
-	// constrain terrain to minimum allowed value and predict height above ground
-	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
-	const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
-
-	// Calculate observation matrix for flow around the vehicle x axis
-	const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
-
-	// Constrain terrain variance to be non-negative
-	_terrain_var = fmaxf(_terrain_var, 0.0f);
-
-	// Cacluate innovation variance
-	_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
-
-	// calculate the kalman gain for the flow x measurement
-	const float Kx = _terrain_var * Hx / _flow_innov_var(0);
-
-	// calculate prediced optical flow about x axis
-	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
-
-	// calculate flow innovation (x axis)
-	_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
-
-	// calculate correction term for terrain variance
-	const float KxHxP =  Kx * Hx * _terrain_var;
-
-	// innovation consistency check
-	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
-	float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
-
-	// do not perform measurement update if badly conditioned
-	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Kx * _flow_innov(0);
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
-		_time_last_flow_terrain_fuse = _time_last_imu;
-	}
-
-	// Calculate observation matrix for flow around the vehicle y axis
-	const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
-
-	// Calculuate innovation variance
-	_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
-
-	// calculate the kalman gain for the flow y measurement
-	const float Ky = _terrain_var * Hy / _flow_innov_var(1);
-
-	// calculate prediced optical flow about y axis
-	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
-
-	// calculate flow innovation (y axis)
-	_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
-
-	// calculate correction term for terrain variance
-	const float KyHyP =  Ky * Hy * _terrain_var;
-
-	// innovation consistency check
-	flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
-
-	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Ky * _flow_innov(1);
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
-		_time_last_flow_terrain_fuse = _time_last_imu;
-	}
-}
-
 void Ekf::controlHaglFakeFusion()
 {
 	if (!_control_status.flags.in_air
@@ -412,9 +254,9 @@ void Ekf::controlHaglFakeFusion()
 void Ekf::resetHaglFake()
 {
 	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+	_state.posd_terrain = _state.pos(2) + _params.rng_gnd_clearance;
 	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
+	P(24, 24) = sq(_params.rng_gnd_clearance);
 	_time_last_hagl_fuse = _time_last_imu;
 }
 
@@ -429,3 +271,95 @@ void Ekf::updateTerrainValidity()
 
 	_hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion);
 }
+
+void Ekf::fuseHaglAllStates()
+{
+	// get a height above ground measurement from the range finder assuming a flat earth
+	const float meas_hagl = _range_sensor.getDistBottom();
+
+	// predict the hagl from the vehicle position and terrain height
+	const float pred_hagl = _state.posd_terrain - _state.pos(2);
+
+	// calculate the innovation
+	_hagl_innov = pred_hagl - meas_hagl;
+
+	// calculate the observation variance adding the variance of the vehicles own height uncertainty
+	const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
+				   + sq(_params.range_noise)
+				   + sq(_params.range_noise_scaler * _range_sensor.getRange());
+
+	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
+	_hagl_innov_var = fmaxf(P(24,24) - 2*P(9,24) + P(9,9) + obs_variance, obs_variance);
+
+	// perform an innovation consistency check and only fuse data if it passes
+	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
+	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
+
+	bool is_fused = false;
+	if (_hagl_test_ratio <= 1.0f) {
+		// calculate the Kalman gain
+		const float HK0 = 1.0f/_hagl_innov_var;
+
+		// calculate the observation Jacobians and Kalman gains
+		SparseVector25f<9,24> Hfusion;
+		Vector25f Kfusion;
+
+		if (_control_status.flags.rng_hgt) {
+			Hfusion.at<9>() = -1.0f;
+			if (_hagl_sensor_status.flags.range_finder || _hagl_sensor_status.flags.flow) {
+				for (uint8_t index=0; index<=23; index++) {
+					Kfusion(index) = HK0*(P(index,24) - P(index,9));
+				}
+
+			} else {
+				for (uint8_t index=0; index<=23; index++) {
+					Kfusion(index) = - HK0*P(index,9);
+				}
+			}
+
+		} else {
+			for (unsigned row=0; row<=23; row++) {
+				// update of all states other than terrain is inhibited
+				Kfusion(row) = 0.0f;
+			}
+		}
+
+		if (_hagl_sensor_status.flags.range_finder) {
+			Hfusion.at<24>() = 1.0f;
+
+			if (_control_status.flags.rng_hgt) {
+				Kfusion(24) = HK0*(P(24,24) - P(24,9));
+
+			} else {
+				Kfusion(24) = HK0*P(24,24);
+			}
+
+		} else {
+			Kfusion(24) = 0.0f;
+		}
+
+		is_fused = measurementUpdate(Kfusion, Hfusion, _hagl_innov);
+	}
+
+	if (is_fused) {
+		// record last successful fusion event
+		if (_hagl_sensor_status.flags.range_finder) {
+			_time_last_hagl_fuse = _time_last_imu;
+			_innov_check_fail_status.flags.reject_hagl = false;
+		}
+
+		if (_control_status.flags.rng_hgt) {
+			_time_last_hgt_fuse = _time_last_imu;
+			_innov_check_fail_status.flags.reject_ver_pos = false;
+		}
+
+	} else {
+		if (_hagl_sensor_status.flags.range_finder) {
+			_innov_check_fail_status.flags.reject_hagl = true;
+		}
+
+		if (_control_status.flags.rng_hgt) {
+			_innov_check_fail_status.flags.reject_ver_pos = true;
+		}
+	}
+}

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -183,7 +183,7 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
 void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
 {
 
-	Vector24f Kfusion;  // Kalman gain vector for any single observation - sequential fusion is used.
+	Vector25f Kfusion;  // Kalman gain vector for any single observation - sequential fusion is used.
 	const unsigned state_index = obs_index + 4;  // we start with vx and this is the 4. state
 
 	// calculate kalman gain K = PHS, where S = 1/innovation variance
@@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		Kfusion(row) = P(row, state_index) / innov_var;
 	}
 
-	SquareMatrix24f KHP;
+	SquareMatrix25f KHP;
 
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 0; column < _k_num_states; column++) {

src/modules/ekf2/EKF2.cpp

@@ -1017,7 +1017,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
 
 	// Get covariances to vehicle odometry
-	float covariances[24];
+	float covariances[25];
 	_ekf.covariances_diagonal().copyTo(covariances);
 
 	// initially set pose covariances to 0

src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp

@@ -21,13 +21,13 @@ void EkfLogger::writeStateToFile()
 		_file << "Timestamp";
 
 		if (_state_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",state[" << i << "]";
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << ",variance[" << i << "]";
 			}
 		}
@@ -52,15 +52,15 @@ void EkfLogger::writeState()
 		_file << time;
 
 		if (_state_logging_enabled) {
-			matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
+			matrix::Vector<float, 25> state = _ekf->getStateAtFusionHorizonAsVector();
 
-			for (int i = 0; i < 24; i++) {
+			for (int i = 0; i < 25; i++) {
 				_file << "," << std::setprecision(3) << state(i);
 			}
 		}
 
 		if (_variance_logging_enabled) {
-			matrix::Vector<float, 24> variance = _ekf->covariances_diagonal();
+			matrix::Vector<float, 25> variance = _ekf->covariances_diagonal();
 
 			for (int i = 0; i < 24; i++) {
 				_file << "," << variance(i);

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -169,7 +169,7 @@ TEST_F(EkfInitializationTest, gyroBias)
 		if (fabsf(accel_bias(2)) > 0.3f) {
 
 			// Print state covariance and correlation matrices for debugging
-			const matrix::SquareMatrix<float, 24> P = _ekf->covariances();
+			const matrix::SquareMatrix<float, 25> P = _ekf->covariances();
 
 			printf("State covariance:\n");
 

src/modules/ekf2/test/test_EKF_terrain_estimator.cpp

@@ -112,9 +112,9 @@ TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion)
 
 	// THEN: By default, both rng and flow aiding are active
 	EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion());
-	EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion());
 	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
-	EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 1e-4);
+	EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 1e-3f);
 
 	// WHEN: rng fusion is disabled
 	_ekf_wrapper.disableTerrainRngFusion();
@@ -138,6 +138,7 @@ TEST_F(EkfTerrainTest, testFlowForTerrainFusion)
 	// GIVEN: flow for terrain enabled but not range finder
 	_ekf_wrapper.enableTerrainFlowFusion();
 	_ekf_wrapper.disableTerrainRngFusion();
+	_ekf_wrapper.enableFlowFusion();
 
 	// WHEN: the sensors do not agree
 	const float rng_height = 1.f;