EKF: correct outputs for IMU offset

EKF/ekf.cpp

@@ -583,9 +583,9 @@ void Ekf::calculateOutputStates()
 	_output_new.quat_nominal = dq * _output_new.quat_nominal;
 	_output_new.quat_nominal.normalize();
 
-	matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
+	_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
 
-	Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
+	Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr;
 	delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
 
 	Vector3f vel_last = _output_new.vel;

EKF/ekf.h

@@ -330,4 +330,5 @@ private:
 
 	// zero the specified range of columns in the state covariance matrix
 	void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
+
 };

EKF/ekf_helper.cpp

@@ -449,3 +449,41 @@ bool Ekf::global_position_is_valid()
 	// TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status
 	return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps);
 }
+
+// perform a vector cross product
+Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
+{
+	Vector3f vecOut;
+	vecOut(0) = vecIn1(1)*vecIn2(2) - vecIn1(2)*vecIn2(1);
+	vecOut(1) = vecIn1(2)*vecIn2(0) - vecIn1(0)*vecIn2(2);
+	vecOut(2) = vecIn1(0)*vecIn2(1) - vecIn1(1)*vecIn2(0);
+	return vecOut;
+}
+
+// calculate the inverse rotation matrix from a quaternion rotation
+Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion quat)
+{
+	float q00 = quat(0) * quat(0);
+	float q11 = quat(1) * quat(1);
+	float q22 = quat(2) * quat(2);
+	float q33 = quat(3) * quat(3);
+	float q01 = quat(0) * quat(1);
+	float q02 = quat(0) * quat(2);
+	float q03 = quat(0) * quat(3);
+	float q12 = quat(1) * quat(2);
+	float q13 = quat(1) * quat(3);
+	float q23 = quat(2) * quat(3);
+
+	Matrix3f dcm;
+	dcm(0,0) = q00 + q11 - q22 - q33;
+	dcm(1,1) = q00 - q11 + q22 - q33;
+	dcm(2,2) = q00 - q11 - q22 + q33;
+	dcm(0,1) = 2.0f * (q12 - q03);
+	dcm(0,2) = 2.0f * (q13 + q02);
+	dcm(1,0) = 2.0f * (q12 + q03);
+	dcm(1,2) = 2.0f * (q23 - q01);
+	dcm(2,0) = 2.0f * (q13 - q02);
+	dcm(2,1) = 2.0f * (q23 + q01);
+
+	return dcm;
+}

EKF/estimator_interface.h

@@ -172,16 +172,30 @@ public:
 			quat[i] = _output_new.quat_nominal(i);
 		}
 	}
-	void copy_velocity(float *vel)
+	// get the velocity of the body frame origin in local NED earth frame
+	void get_velocity(float *vel)
 	{
+		// calculate the average angular rate across the last IMU update
+		Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f/_imu_sample_new.delta_ang_dt);
+		// calculate the velocity of the relative to the body origin
+		// Note % operator has been overloaded to performa cross product
+		Vector3f vel_imu_rel_body = cross_product(ang_rate , _params.imu_pos_body);
+		// rotate the relative velocty into earth frame and subtract from the EKF velocity
+		// (which is at the IMU) to get velocity of the body origin
+		Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body;
+		// copy to output
 		for (unsigned i = 0; i < 3; i++) {
-			vel[i] = _output_new.vel(i);
+			vel[i] = vel_earth(i);
 		}
 	}
-	void copy_position(float *pos)
+	// get the position of the body frame origin in local NED earth frame
+	void get_position(float *pos)
 	{
+		// rotate the position of the IMU relative to the boy origin into earth frame
+		Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
+		// subtract from the EKF position (which is at the IMU) to get position at the body origin
 		for (unsigned i = 0; i < 3; i++) {
-			pos[i] = _output_new.pos(i);
+			pos[i] = _output_new.pos(i) - pos_offset_earth(i);
 		}
 	}
 	void copy_timestamp(uint64_t *time_us)
@@ -218,6 +232,7 @@ protected:
 	outputSample _output_sample_delayed;	// filter output on the delayed time horizon
 	outputSample _output_new;	// filter output on the non-delayed time horizon
 	imuSample _imu_sample_new;	// imu sample capturing the newest imu data
+	Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
 
 	uint64_t _imu_ticks;	// counter for imu updates
 
@@ -273,4 +288,10 @@ protected:
 	// this is the previous status of the filter control modes - used to detect mode transitions
 	filter_control_status_u _control_status_prev;
 
+	// perform a vector cross product
+	Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2);
+
+	// calculate the inverse rotation matrix from a quaternion rotation
+	Matrix3f quat_to_invrotmat(const Quaternion quat);
+
 };