Added complementary filter for orientation tracking

Source/LuckyWorld/EKFilter.cpp

@@ -237,15 +237,13 @@ bool EKFilter::readBaro()
 	return false;
 }
 
-void EKFilter::GetState(StateElements& state_struct)
+void EKFilter::GetState(OutputState& state_struct)
 {
 	state_struct.quat = State.quat;
 	state_struct.position = State.position;
 	state_struct.velocity = State.velocity;
-	state_struct.gyro_bias = State.gyro_bias;
-	state_struct.accel_bias = State.accel_bias;
-	state_struct.earth_magfield = State.earth_magfield;
-	state_struct.body_magfield = State.body_magfield;
+	state_struct.pqr = recentGyro.data_dt / recentGyro.dt;
+	state_struct.acceleration = recentAccel.data_dt / recentAccel.dt;
 }
 
 bool EKFilter::InitialiseFilter()
@@ -318,9 +316,9 @@ void EKFilter::InitialzeVariables()
 	//_magNoise = 0.1f;
 
 	_gyrNoise = 1.5E-04f;
-	_accNoise = 3.5E-04f;
+	//_accNoise = 3.5E-04f;
 	//_gyrNoise = 0.5f;
-	//_accNoise = 1.0f;
+	_accNoise = 1.0f;
 
 	//_gpsVertVelNoise = 0.7;
 	//_gpsHorizVelNoise = 0.5;
@@ -328,8 +326,8 @@ void EKFilter::InitialzeVariables()
 	//_gpsVertPosNoise = 3.0;
 	_gpsVertVelNoise = 0.5f;
 	_gpsHorizVelNoise = 0.5f;
-	_gpsHorizPosNoise = 1.0f;
-	_gpsVertPosNoise = 1.0f;
+	_gpsHorizPosNoise = 0.5f;
+	_gpsVertPosNoise = 0.5f;
 
 	_baroAltNoise = 2.0f;
 
@@ -417,22 +415,13 @@ void EKFilter::CompFilterUpdate()
 	FVector accel_euler(accel_roll, accel_pitch, nq_yaw);
 	//UE_LOG(LogTemp, Warning, TEXT("Quat accel euler: %f % f %f!"), accel_euler.X, accel_euler.Y, accel_euler.Z);
 
-	float dt = 0.5 * (recentGyro.dt + recentAccel.dt);
-	float tau = 0.1;
-	float gyro_gain = tau / (tau + dt);
-	float acc_gain = dt / (tau + dt);
-	FVector EulerGyroAccel = nq_euler * gyro_gain + accel_euler * acc_gain;
-	//CFquat.InitializeFromEulerAngles(EulerGyroAccel.X, EulerGyroAccel.Y, EulerGyroAccel.Z);
-	//UE_LOG(LogTemp, Warning, TEXT("Combined euler: %f % f %f!"), EulerGyroAccel.X, EulerGyroAccel.Y, EulerGyroAccel.Z);
+	//float dt = 0.5 * (recentGyro.dt + recentAccel.dt);
+	//float tau = 0.2;
+	//float gyro_gain = tau / (tau + dt);
+	//float acc_gain = dt / (tau + dt);
+	//nq_euler = nq_euler * gyro_gain + accel_euler * acc_gain;
 
-	FQuaternion gyro_accel_quat;
-	gyro_accel_quat.InitializeFromEulerAngles(EulerGyroAccel.X, EulerGyroAccel.Y, EulerGyroAccel.Z);
-
-	float roll = State.quat.GetRoll();
-	float pitch = State.quat.GetPitch();
-	float yaw = State.quat.GetYaw();
 	Matrix33 DCM;
-	//DCM.FromEuler(roll, pitch, yaw);
 	DCM.FromEuler(nq_euler.X, nq_euler.Y, nq_euler.Z);
 	
 	FVector mf_pred = DCM * recentMag.data;
@@ -442,6 +431,7 @@ void EKFilter::CompFilterUpdate()
 	FVector d_euler = q_between.Euler() * (PI/180);
 	//UE_LOG(LogTemp, Warning, TEXT("Correcting euler: %f % f %f!"), d_euler.X, d_euler.Y, d_euler.Z);
 
+	
 	mf_pred.Normalize();
 	mf.Normalize();
 	double theta = 0;
@@ -474,8 +464,9 @@ void EKFilter::CompFilterUpdate()
 		FQuaternion mf_quat_dq = FQuaternion::from_axis_angle(axis, theta);
 		FQuaternion mf_quat = CFquat * mf_quat_dq;
 		mf_euler = { mf_quat.GetRoll(), mf_quat.GetPitch(), mf_quat.GetYaw() };
+		//UE_LOG(LogTemp, Warning, TEXT(" MF Quat euler: %f % f %f!"), mf_euler.X, mf_euler.Y, mf_euler.Z);
 	}
-	//UE_LOG(LogTemp, Warning, TEXT(" MF Quat euler: %f % f %f!"), mf_euler.X, mf_euler.Y, mf_euler.Z);
+	
 	FVector final_euler;
 	if (mf_euler.Size() > 1e-4)
 	{
@@ -491,11 +482,12 @@ void EKFilter::CompFilterUpdate()
 
 void EKFilter::UpdateState()
 {
-	FVector delAngles = recentGyro.data_corrected;
-	delAngles.X = fabs(delAngles.X) > 1e-3 ? delAngles.X : 0.0;
-	delAngles.Y = fabs(delAngles.Y) > 1e-3 ? delAngles.Y : 0.0;
-	delAngles.Z = fabs(delAngles.Z) > 1e-3 ? delAngles.Z : 0.0;
+	//FVector delAngles = recentGyro.data_corrected;
+	//delAngles.X = fabs(delAngles.X) > 1e-3 ? delAngles.X : 0.0;
+	//delAngles.Y = fabs(delAngles.Y) > 1e-3 ? delAngles.Y : 0.0;
+	//delAngles.Z = fabs(delAngles.Z) > 1e-3 ? delAngles.Z : 0.0;
 	//UE_LOG(LogTemp, Warning, TEXT("Using delta angles: %f % f %f!"), delAngles.X, delAngles.Y, delAngles.Z);
+	FVector delAngles = (recentGyro.data - State.gyro_bias) * recentGyro.dt;
 	FQuaternion dq = FQuaternion::from_axis_angle(delAngles);
 	FQuaternion nq = State.quat * dq;
 
@@ -1170,7 +1162,7 @@ void EKFilter::FuseMagnetometer()
 				}
 				else
 				{
-					//UE_LOG(LogTemp, Display, TEXT("Magnetometer fusion KHP[%d][%d], P[%d][%d]  : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]);
+					UE_LOG(LogTemp, Display, TEXT("Magnetometer fusion KHP[%d][%d], P[%d][%d]  : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]);
 					healthyFusion = false;
 				}
 			}
@@ -1441,6 +1433,11 @@ void EKFilter::VelPosFusion()
 			Kfusion[i] = P[i][stateIndex] * SK;
 		}
 
+		//for (uint8_t i = 16; i < NumStates; i++)
+		//{
+			//Kfusion[i] = 0.0;
+		//}
+
 		for (uint8_t i = 0; i < NumStates; i++)
 		{
 			for (uint8_t j = 0; j < NumStates; j++)
@@ -1460,7 +1457,7 @@ void EKFilter::VelPosFusion()
 				}
 				else
 				{
-					//UE_LOG(LogTemp, Display, TEXT("Velocity Position KHP[%d][%d], P[%d][%d]  : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]);
+					UE_LOG(LogTemp, Display, TEXT("Velocity Position KHP[%d][%d], P[%d][%d]  : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]);
 					healthyFusion = false;
 				}
 			}

Source/LuckyWorld/EKFilter.h

@@ -38,6 +38,15 @@ struct StateElements
 	FVector     body_magfield;
 };
 
+struct OutputState
+{
+	FQuaternion quat;
+	FVector     velocity = { 0, 0, 0 };
+	FVector     position = { 0, 0, 0 };
+	FVector     pqr;
+	FVector     acceleration;
+};
+
 class LUCKYWORLD_API EKFilter
 {
 public:
@@ -56,7 +65,7 @@ public:
 	const bool IsInitialised() const { return Initialised; };
 	bool InitialiseFilter();
 	void Update();
-	void GetState(StateElements& state_struct);
+	void GetState(OutputState& state_struct);
 private:
 	StateElements State;
 	Vector22 StateArray;

Source/LuckyWorld/MultiRotorDrone.cpp

@@ -83,13 +83,9 @@ void AMultiRotorDrone::SetCurrentState()
 		TargetPosition = State.Position;
 		TargetYaw = State.Orientation.Z;
 		State.initialised = true;
-		return;
 	}
-	
-	FVector accel = (State.Velocity - prev_velocity) / MjStep;
-	State.Acceleration = accel;
-	FVector EulerDt = (State.Orientation - prev_euler) / MjStep;
-	EulerDt2PQR(EulerDt);
+	State.Acceleration = EstState.acceleration;
+	PQR = EstState.pqr;
 }
 
 void AMultiRotorDrone::EulerDt2PQR(FVector& EulerDt)

Source/LuckyWorld/MultiRotorDrone.h

@@ -128,7 +128,7 @@ protected:
 	bool KeepXYPosition = false;
 
 	FlyingMode Mode = MANUAL;
-
+	bool use_true_state = true;
 
 
 	struct PID_Controller
@@ -288,9 +288,8 @@ protected:
 
 	FTrueDroneState TrueState;
 	FTrueDroneState State;
-	StateElements EstState;
+	OutputState EstState;
 
-	bool use_true_state = true;
 	bool AltitudeManualControl = true;
 	bool XYmanualControl = true;