Flight controller sensors and EKF

Plugins/LuckyMujoco/Source/LuckyMujoco/Private/LuckyMujoco.cpp

@@ -11,10 +11,10 @@ DEFINE_LOG_CATEGORY(LogMujoco);
 
 void FLuckyMujocoModule::StartupModule()
 {
-
+	FString BaseDir = IPluginManager::Get().FindPlugin(TEXT("LuckyMujoco"))->GetBaseDir();
 
 #if PLATFORM_WINDOWS
-	FString Library = FPaths::Combine(FPaths::ProjectDir(), TEXT("Binaries/Win64/mujoco.dll"));
+	FString Library = FPaths::Combine(*BaseDir, TEXT("Source/ThirdParty/Mujoco/bin/mujoco.dll"));
 	MujocoHandle = !Library.IsEmpty() ? FPlatformProcess::GetDllHandle(*Library) : nullptr;
 	
 	if (MujocoHandle == nullptr)

Source/LuckyWorld/Barometer.cpp

@@ -0,0 +1,66 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Barometer.h"
+
+// Sets default values for this component's properties
+UBarometer::UBarometer()
+{
+	// Set this component to be initialized when the game starts, and to be ticked every frame.  You can turn these features
+	// off to improve performance if you don't need them.
+	PrimaryComponentTick.bCanEverTick = true;
+
+	// ...
+}
+
+
+// Called when the game starts
+void UBarometer::BeginPlay()
+{
+	Super::BeginPlay();
+
+	// ...
+	FVector pos = GetComponentLocation();
+	alt_start = pos.Z / 100;
+}
+
+
+// Called every frame
+void UBarometer::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
+{
+	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+
+	// ...
+	UpdateGroundTruth();
+	UpdateSensorData(MjStep);
+}
+
+void UBarometer::UpdateGroundTruth()
+{
+	FVector pos = GetComponentLocation();
+	double alt_pos_rel = pos.Z / 100 - alt_start;
+	double alt_amsl = default_home_amsl + alt_pos_rel;
+	temperature_local = temperature_msl - lapse_rate * alt_amsl;
+	pressure_ratio = powf(temperature_msl / temperature_local, 5.256f);
+	absolute_pressure = pressure_msl / pressure_ratio;
+}
+
+void UBarometer::UpdateSensorData(double dt)
+{
+	double noise = RandomGenerator.RandNorm(0, 1);
+	baro_drift_pa += baro_drift_pa_per_sec * dt;
+	absolute_pressure_noisy = absolute_pressure + noise + baro_drift_pa;
+	absolute_pressure_noisy = absolute_pressure_noisy * 0.01f;
+
+	double pressure_ratio_noisy = pressure_msl / (absolute_pressure_noisy * 100.0f);
+	double temperature_local_noisy = temperature_msl / powf(pressure_ratio_noisy, (1.0f / 5.256f));
+	altitude = ((temperature_msl - temperature_local_noisy) / lapse_rate) - default_home_amsl;
+	altitude += alt_start;
+
+	baroData.altitude = altitude;
+	baroData.pressure = absolute_pressure_noisy;
+	baroData.dt = dt;
+
+	double data_array[3] = { baroData.pressure, baroData.altitude, baroData.dt };
+	DataBuffer.Append(data_array);
+}

Source/LuckyWorld/Barometer.h

@@ -0,0 +1,62 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "Components/SceneComponent.h"
+#include "Random.h"
+#include "Barometer.generated.h"
+
+USTRUCT()
+struct FBaroData
+{
+	GENERATED_BODY()
+	double pressure;
+	double altitude;
+	double dt;
+};
+
+UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) )
+class LUCKYWORLD_API UBarometer : public USceneComponent
+{
+	GENERATED_BODY()
+
+public:
+	// Sets default values for this component's properties
+	UBarometer();
+
+	TArray<double> DataBuffer;
+
+protected:
+	// Called when the game starts
+	virtual void BeginPlay() override;
+
+	FBaroData baroData;
+	double default_home_amsl = 488.0;
+	//double default_home_amsl = 0;
+	double temperature_msl = 288.15;
+	double pressure_msl = 101325.0;
+	double lapse_rate = 0.0065;
+	double air_dencity_msl = 1.225;
+	double absolute_zero = -273.15;
+
+	double baro_drift_pa_per_sec = 0;
+	double baro_drift_pa = 0;
+
+	double alt_start;
+	double absolute_pressure;
+	double absolute_pressure_noisy;
+	double temperature_local;
+	double pressure_ratio;
+	double altitude;
+
+	void UpdateGroundTruth();
+	void UpdateSensorData(double dt);
+public:
+	// Called every frame
+	virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
+private:
+	Random RandomGenerator;
+	//float MjStep = 1 * 0.001667;
+	float MjStep = 1 * 0.001;
+};

Source/LuckyWorld/EKF.h

@@ -0,0 +1,317 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "FQuaternion.h"
+#include "Matrix33.h"
+#include "Location.h"
+#include "IMUSensor.h"
+
+#define VEL_STATE_MIN_VARIANCE 1E-4
+
+#ifndef GRAVITY
+#define GRAVITY      9.81
+#endif
+
+/**
+ *
+ */
+
+UENUM()
+enum class SensorType : uint8
+{
+	GYRO = 1,
+	ACCEL,
+	MAG,
+	GPS,
+	BARO
+};
+
+struct FStateElements
+{
+	float      quat[4];
+	FVector    velocity;
+	FVector    position;
+	FVector    gyro_bias;
+	FVector    accel_bias;
+	FVector    earth_magfield;
+	FVector    body_magfield;
+};
+
+class LUCKYWORLD_API EKF
+{
+public:
+	typedef float Matrix24[24][24];
+	typedef float Vector28[28];
+	typedef float Vector26[26];
+	typedef float Vector22[22];
+	typedef float Vector9[9];
+	typedef float Vector5[5];
+	typedef float Vector6[6];
+	typedef float Matrix22[22][22];
+
+	EKF();
+	~EKF();
+
+	//bool TestSensorReading();
+	bool InitialiseFilter();
+	void GetState(FStateElements& state_struct);
+	void Update(bool update);
+	void RegisterBuffer(SensorType sensor_type, TArray<double>& buffer);
+	const bool IsInitialised() const { return Initialised; };
+private:
+	bool Initialised = false;
+	bool GyroBufferSet = false;
+	bool AccelBufferSet = false;
+	bool MagBufferSet = false;
+	bool GPSBufferSet = false;
+	bool BaroBufferSet = false;
+
+	struct SensorReading
+	{
+		FVector data;
+		FVector data_corrected;
+		double dt;
+	};
+
+	struct GPSdata
+	{
+		double LonDeg;
+		double LatDeg;
+		double Alt;
+		double VelocityEast;
+		double VelocityNorth;
+		double VelocityUP;
+		double dt;
+	};
+
+	struct BaroData
+	{
+		double pressure;
+		double altitude;
+		double dt;
+	};
+
+	struct IMUdata
+	{
+		FVector data;
+		double dt;
+	};
+
+	union
+	{
+		IMUdata RecentIMUData;
+		double IMUDataArray[4];
+	};
+
+	union
+	{
+		BaroData recentBaro;
+		double BaroDataArray[3];
+	};
+
+	union
+	{
+		GPSdata recentGPS;
+		double GPSDataArray[7];
+	};
+
+	FQuaternion StateQuat;
+	FStateElements State;
+	Vector22 statesArray;
+	TArray<double>* GyroDataBuffer;
+	TArray<double>* AccelDataBuffer;
+	TArray<double>* MagDataBuffer;
+	TArray<double>* GPSDataBuffer;
+	TArray<double>* BaroDataBuffer;
+	float gravity = 9.80665f;
+	double mf_inclination = -30.89 * (PI / 180);
+	double mf_declination = -4.016 * (PI / 180);
+	double mf_intencity = 0.3184;
+	double GPSHorizontalAccuracy = 1.6;
+	double GPSVerticalAccuracy = 3;
+	double GPSVelocityAccuracy = 0.1;
+
+	//struct 
+	//{
+		//float pos;
+		//float vel;
+		//float acc;
+	//} vertCompFiltState;
+
+	//struct yaw_elements
+	//{
+		//float yaw_angle;
+		//float yaw_error;
+	//}yawAngData;
+
+	int num_states = 22;
+
+	//state_elements State;
+	uint8_t stateIndexLim = 21;
+
+	SensorReading recentGyro;
+	SensorReading recentAccel;
+	SensorReading recentMag;
+	TArray<SensorReading> GyroReadings;
+	TArray<SensorReading> AccelReadings;
+	TArray<SensorReading> MagReadings;
+	TArray<GPSdata> GPSreadings;
+	TArray<BaroData> BaroReadings;
+
+	//float YawAngle;
+	//float YawAngleErr;
+
+	FVector accelBias;
+	FVector gyroBias;
+	FVector magOffsets;
+
+	Matrix33 prevTnb;
+	Matrix33 Tb2l;
+	Matrix33 Tl2b;
+
+	Matrix22 P;
+	Matrix22 nextP;
+	Vector26 Kfusion;
+	Vector6 velPosObs;
+	Matrix22 KH;
+	Matrix22 KHP;
+
+	Vector6 innovVelPos;
+	Vector6 varInnovVelPos;
+
+	Location EKF_origin;
+
+	//FVector velDotNED;
+	//FVector velDotNEDfilt;
+	FVector delAngBodyOF;
+	FVector innovMag;
+	float innovYaw;
+	FVector varInnovMag;
+	FVector gpsVelInnov;
+	FVector gpsVelVarInnov;
+
+	float gpsSpdAccuracy;
+	float gpsPosAccuracy;
+	float gpsHgtAccuracy;
+	float gpsNoiseScaler;
+	float posDownObsNoise;
+	float magVarRateScale;
+	float hgtMea;
+	float tiltErrorVariance;
+	float gpsNEVelVarAccScale;
+	float baroHgtOffset;
+	//float ekfOriginHgtVar;
+	float gpsPosVarAccScale;
+
+	bool validOrigin = false;
+	bool magFieldLearned = false;
+	bool onGround = true;
+
+	SensorType HeightSource = SensorType::GPS;
+
+	int vertVelVarClipCounter;
+
+	//double accNavMag;
+	//double accNavMagHoriz;
+	double delTimeOF;
+	double cov_dt;
+	double hgtRate;
+	//double ekfGpsRefHgt;
+
+	bool needMagBodyVarReset = true;
+	bool needEarthBodyVarReset = true;
+
+	bool badIMUdata = false;
+
+	float _gyroBiasProcessNoise;
+	float _accelBiasProcessNoise;
+	float _magEarthProcessNoise;
+	float _magBodyProcessNoise;
+	float _magNoise;
+	float _gyrNoise;
+	float _accNoise;
+	float _gpsVertVelNoise;
+	float _gpsVertPosNoise;
+	float _baroAltNoise;
+	float _gpsHorizVelNoise;
+	float _gpsHorizPosNoise;
+	float _accBiasLim;
+	float _yawNoise;
+	float _baroGndEffectDeadZone;
+
+	float accelFiltRatio{ 10.0 };
+	float tiltGain{ 0.2 };
+	FVector ahrsAccel = { 0.0, 0.0, -GRAVITY };
+	float ahrs_accel_norm;
+
+	float accel_gain = 1.0f;
+
+	void InitialiseVariables();
+	bool readIMUData();
+	bool GetNextData(TArray<double>* sensor_buffer, double* data_array, int num);
+	bool readMagData();
+	bool readGPS();
+	bool readBaro();
+	float GetMagYawAngle();
+
+	//template<typename T>
+	//bool readNewData(TArray<T>& data_array, T& recent_data);
+	void CorrectSensorData(SensorReading& sensor_data, FVector bias);
+	void UpdateVelPosQuat();
+	void CovariancePrediction(FVector* rotVarVectPtr);
+	void ConstrainVariances();
+	void ConstrainStates();
+	void zeroCols(Matrix22& covMat, uint8_t first, uint8_t last);
+	void zeroRows(Matrix22& covMat, uint8_t first, uint8_t last);
+	void FuseDeclination(float declErr);
+	void ForceSymmetry();
+	float constrain_value(const float amt, const float low, const float high) const;
+	bool setOrigin(const Location& loc);
+	void alignMagStateDeclination();
+	void FillStateStruct();
+	void FillStateArray();
+	float MagDeclination();
+	void ResetVelocity();
+	void ResetPosition();
+	void ResetHeight();
+	void CovarianceInit();
+	void calcTiltErrorVariance();
+	float sq(const float v) const { return v * v; };
+	float radians(float deg) { return deg * (PI / 180); };
+	float wrap_2PI(const float radian)
+	{
+		float res = fmodf(radian, 2 * PI);
+		if (res < 0) {
+			res += 2 * PI;
+		}
+		return res;
+	}
+	float wrap_PI(const float radian)
+	{
+		float res = wrap_2PI(radian);
+		if (res > PI) {
+			res -= 2 * PI;
+		}
+		return res;
+	}
+	void MagFusion();
+	void setYawFromMag();
+	void resetQuatStateYawOnly(float yaw, float yawVariance);
+	void resetMagFieldStates();
+	void FuseMagnetometer();
+	void VelPosFusion();
+	void CalculateVelInnovationsAndVariances(const FVector& velocity, float noise, float accel_scale, FVector& innovations, FVector& variances) const;
+	void selectHeightForFusion();
+	void FuseVelPosNED();
+	void fuseEulerYaw();
+	void quat2arr() { for (int i = 0; i < 4; i++) statesArray[i] = StateQuat(i); }
+	void arr2quat() { for (int i = 0; i < 4; i++) StateQuat(i) = statesArray[i]; }
+	void GetAccelGain(FVector delVel);
+
+	inline bool is_positive(float fval)
+	{
+		return (static_cast<float>(fval) >= FLT_EPSILON);
+	}
+};

Source/LuckyWorld/EKFilter.h

@@ -0,0 +1,258 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "FQuaternion.h"
+#include "Matrix33.h"
+#include "Location.h"
+
+/**
+ * 
+ */
+
+#ifndef GRAVITY
+#define GRAVITY      9.81
+#endif
+
+UENUM()
+enum class Sensor : uint8
+{
+	GYRO = 1,
+	ACCEL,
+	MAG,
+	GPS,
+	BARO
+};
+
+struct StateElements
+{
+	FQuaternion quat;
+	FVector     velocity = { 0, 0, 0 };
+	FVector     position = { 0, 0, 0 };
+	//FVector     gyro_bias = { 0.349, 0.349, 0.349 };
+	//FVector     accel_bias = { 0.05, 0.05, 0.08 };
+	FVector     gyro_bias = { 0, 0, 0 };
+	FVector     accel_bias = { 0, 0, 0 };
+	FVector     earth_magfield;
+	FVector     body_magfield;
+};
+
+class LUCKYWORLD_API EKFilter
+{
+public:
+	typedef float Matrix24[24][24];
+	typedef float Vector28[28];
+	typedef float Vector26[26];
+	typedef float Vector22[22];
+	typedef float Vector9[9];
+	typedef float Vector5[5];
+	typedef float Vector6[6];
+	typedef float Matrix22[22][22];
+	EKFilter();
+	~EKFilter();
+
+	void RegisterBuffer(Sensor sensor_type, TArray<double>& buffer);
+	const bool IsInitialised() const { return Initialised; };
+	bool InitialiseFilter();
+	void Update();
+	void GetState(StateElements& state_struct);
+private:
+	StateElements State;
+	Vector22 StateArray;
+	bool Initialised = false;
+	bool GyroBufferSet = false;
+	bool AccelBufferSet = false;
+	bool MagBufferSet = false;
+	bool GPSBufferSet = false;
+	bool BaroBufferSet = false;
+
+	struct IMUSensorData
+	{
+		FVector data = { 0, 0, 0 };
+		FVector prev_data = { 0, 0, 0 };
+		FVector data_dt = { 0, 0, 0 };
+		FVector prev_data_dt = { 0, 0, 0 };
+		FVector data_corrected = { 0, 0, 0 };
+		double dt;
+	};
+
+	struct GPSdata
+	{
+		double LonDeg;
+		double LatDeg;
+		double Alt;
+		double VelocityEast;
+		double VelocityNorth;
+		double VelocityUP;
+		double dt;
+	};
+
+	struct BaroData
+	{
+		double pressure;
+		double altitude;
+		double dt;
+	};
+
+	IMUSensorData recentGyro;
+	IMUSensorData recentAccel;
+	IMUSensorData recentMag;
+	GPSdata recentGPS;
+	BaroData recentBaro;
+
+	TArray<double>* GyroDataBuffer;
+	TArray<double>* AccelDataBuffer;
+	TArray<double>* MagDataBuffer;
+	TArray<double>* GPSDataBuffer;
+	TArray<double>* BaroDataBuffer;
+
+	Location Origin;
+
+	int32 NumStates = 22;
+
+	Matrix33 prevTnb;
+
+	Matrix22 P;
+	Matrix22 nextP;
+	Vector26 Kfusion;
+	Matrix22 KH;
+	Matrix22 KHP;
+
+	Vector6 innovVelPos;
+	Vector6 varInnovVelPos;
+
+	void InitialzeVariables();
+
+	float _gyroBiasProcessNoise;
+	float _accelBiasProcessNoise;
+	float _magEarthProcessNoise;
+	float _magBodyProcessNoise;
+	float _magNoise;
+	float _gyrNoise;
+	float _accNoise;
+	float _gpsVertVelNoise;
+	float _gpsVertPosNoise;
+	float _baroAltNoise;
+	float _gpsHorizVelNoise;
+	float _gpsHorizPosNoise;
+	float _accBiasLim;
+	float _yawNoise;
+	float _yawInnovGate;
+	float _magInnovGate;
+	float _gpsPosInnovGate;
+	float _gpsVelInnovGate;
+	float _hgtInnovGate;
+	float _gpsGlitchRadiusMax;
+
+	float magVarRateScale = 0.005f;
+	float gpsNEVelVarAccScale = 0.05f;
+	float gpsPosVarAccScale = 0.05f;
+	float gpsDVelVarAccScale = 0.07f;
+
+	FVector innovMag;
+	FVector varInnovMag;
+	FVector velDotNED;
+	FVector velDotNEDfilt;
+
+	float MagFieldDeclination;
+
+	float accNavMag;
+	float accNavMagHoriz;
+
+	float tiltErrorVariance;
+	float accel_gain = 1;
+	FVector ahrsAccel = { 0, 0, -GRAVITY };
+
+	Sensor HeightSource = Sensor::GPS;
+
+	void FillStateStruct();
+	void FillStateArray();
+	void quat2arr() { for (int i = 0; i < 4; i++) StateArray[i] = State.quat(i); }
+	void arr2quat() { for (int i = 0; i < 4; i++) State.quat(i) = StateArray[i]; }
+	float wrap_2PI(const float radian)
+	{
+		float res = fmodf(radian, 2 * PI);
+		if (res < 0) {
+			res += 2 * PI;
+		}
+		return res;
+	}
+	float wrap_PI(const float radian)
+	{
+		float res = wrap_2PI(radian);
+		if (res > PI) {
+			res -= 2 * PI;
+		}
+		return res;
+	}
+	float sq(const float v) const { return v * v; };
+	float radians(float deg) { return deg * (PI / 180); };
+	void zeroCols(Matrix22& covMat, uint8_t first, uint8_t last)
+	{
+		uint8_t row;
+		for (row = 0; row <= 23; row++)
+		{
+			uint8_t col;
+			for (col = first; col <= last; col++)
+			{
+				covMat[row][col] = 0;
+			}
+		}
+	}
+	void zeroRows(Matrix22& covMat, uint8_t first, uint8_t last)
+	{
+		uint8_t row;
+		for (row = first; row <= last; row++)
+		{
+			uint8_t col;
+			for (col = 0; col <= 23; col++)
+			{
+				covMat[row][col] = 0;
+			}
+		}
+	}
+	float constrain_value(const float amt, const float low, const float high) const
+	{
+		if (isnan(amt))
+		{
+			return (low + high) / 2;
+		}
+
+		if (amt < low) {
+			return low;
+		}
+
+		if (amt > high) {
+			return high;
+		}
+
+		return amt;
+	}
+	inline bool is_positive(float fval)
+	{
+		return (static_cast<float>(fval) >= FLT_EPSILON);
+	}
+
+	bool readIMUData();
+	bool GetNextData(TArray<double>* sensor_buffer, double* data_array, int num);
+	bool readMagData();
+	bool readGPS();
+	bool readBaro();
+	float MagDeclination();
+	float GetMagYawAngle();
+	void setYawFromMag();
+	void calcTiltErrorVariance();
+	void ConstrainVariances();
+	void ForceSymmetry();
+	void GetAccelGain(FVector delVel);
+	void CovarianceInit();
+	void UpdateState();
+	void MagFusion();
+	void FuseDeclination(float declErr);
+	void VelPosFusion();
+	void fuseEulerYaw();
+	void FuseMagnetometer();
+	void YawAngleFusion();
+	void CovariancePrediction(FVector* rotVarVectPtr);
+};

Source/LuckyWorld/FQuaternion.cpp

@@ -0,0 +1,230 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "FQuaternion.h"
+#include "Matrix33.h"
+
+FQuaternion::FQuaternion(FQuat ue_quat)
+{
+	data[0] = -ue_quat.W;
+	data[1] = ue_quat.X;
+	data[2] = -ue_quat.Y;
+	data[2] = ue_quat.Z;
+}
+
+FQuaternion::FQuaternion()
+{
+	data[0] = 1;
+	data[1] = 0;
+	data[2] = 0;
+	data[2] = 0;
+}
+
+FQuaternion::FQuaternion(double w, double x, double y, double z)
+{
+	data[0] = w;
+	data[1] = x;
+	data[2] = y;
+	data[3] = z;
+}
+double FQuaternion::GetNorm()
+{
+	double norm = sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3]);
+	return norm;
+}
+
+void FQuaternion::Normalize()
+{
+	double norm = GetNorm();
+	if (norm == 0.0 || fabs(norm - 1.000) < 1e-10) return;
+
+	double rnorm = 1.0 / norm;
+
+	data[0] *= rnorm;
+	data[1] *= rnorm;
+	data[2] *= rnorm;
+	data[3] *= rnorm;
+}
+
+FQuaternion::~FQuaternion()
+{
+}
+
+FQuaternion FQuaternion::Inverse()
+{
+	double norm = GetNorm();
+	if (norm == 0.0)
+		return *this;
+	double rNorm = 1.0 / norm;
+	return FQuaternion(data[0] * rNorm, -data[1] * rNorm, -data[2] * rNorm, -data[3] * rNorm);
+}
+
+FQuaternion FQuaternion::Conjugate()
+{
+	return FQuaternion(data[0], -data[1], -data[2], -data[3]);
+}
+
+FQuaternion  FQuaternion::operator*(const FQuaternion& q) const
+{
+	return FQuaternion(data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
+		data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
+		data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
+		data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
+}
+
+double& FQuaternion::operator()(unsigned int idx)
+{
+	return data[idx];
+}
+
+Matrix33 FQuaternion::GetMatrix()
+{
+	double q0 = data[0];
+	double q1 = data[1];
+	double q2 = data[2];
+	double q3 = data[3];
+
+	double q0q0 = q0 * q0;
+	double q1q1 = q1 * q1;
+	double q2q2 = q2 * q2;
+	double q3q3 = q3 * q3;
+	double q0q1 = q0 * q1;
+	double q0q2 = q0 * q2;
+	double q0q3 = q0 * q3;
+	double q1q2 = q1 * q2;
+	double q1q3 = q1 * q3;
+	double q2q3 = q2 * q3;
+
+	double m0 = q0q0 + q1q1 - q2q2 - q3q3;
+	double m1 = 2.0 * (q1q2 + q0q3);
+	double m2 = 2.0 * (q1q3 - q0q2);
+	double m3 = 2.0 * (q1q2 - q0q3);
+	double m4 = q0q0 - q1q1 + q2q2 - q3q3;
+	double m5 = 2.0 * (q2q3 + q0q1);
+	double m6 = 2.0 * (q1q3 + q0q2);
+	double m7 = 2.0 * (q2q3 - q0q1);
+	double m8 = q0q0 - q1q1 - q2q2 + q3q3;
+
+	Matrix33 M(m0, m1, m2, m3, m4, m5, m6, m7, m8);
+	return M;
+}
+
+FVector  FQuaternion::GetEulerRad()
+{
+	//Matrix33 M = GetMatrix();
+	//FVector Euler = M.GetEuler();
+	//return Euler;
+	float roll = GetRoll();
+	float pitch = GetPitch();
+	float yaw = GetYaw();
+	return FVector(roll, pitch, yaw);
+}
+
+FVector FQuaternion::GetEulerDeg()
+{
+	FVector Euler = GetEulerRad();
+	return Euler * (180 / PI);
+}
+
+float  FQuaternion::GetRoll()
+{
+	float q1 = data[0];
+	float q2 = data[1];
+	float q3 = data[2];
+	float q4 = data[3];
+
+	return (atan2f(2.0f * (q1 * q2 + q3 * q4), 1.0f - 2.0f * (q2 * q2 + q3 * q3)));
+}
+
+float FQuaternion::GetPitch()
+{
+	float q1 = data[0];
+	float q2 = data[1];
+	float q3 = data[2];
+	float q4 = data[3];
+
+	return safe_asin(2.0f * (q1 * q3 - q4 * q2));
+}
+
+float FQuaternion::GetYaw()
+{
+	float q1 = data[0];
+	float q2 = data[1];
+	float q3 = data[2];
+	float q4 = data[3];
+
+	return atan2f(2.0f * (q1 * q4 + q2 * q3), 1.0f - 2.0f * (q3 * q3 + q4 * q4));
+}
+
+float FQuaternion::safe_asin(const float v)
+{
+	if (isnan(v)) {
+		return 0.0f;
+	}
+	if (v >= 1.0f) {
+		return static_cast<float>(PI/2);
+	}
+	if (v <= -1.0f) {
+		return static_cast<float>(-PI/2);
+	}
+	return asinf(v);
+}
+
+void FQuaternion::InitializeFromEulerAngles(double roll, double pitch, double yaw)
+{
+	double thtd2 = 0.5 * pitch;
+	double psid2 = 0.5 * yaw;
+	double phid2 = 0.5 * roll;
+
+	double Sthtd2 = sin(thtd2);
+	double Spsid2 = sin(psid2);
+	double Sphid2 = sin(phid2);
+
+	double Cthtd2 = cos(thtd2);
+	double Cpsid2 = cos(psid2);
+	double Cphid2 = cos(phid2);
+
+	double Cphid2Cthtd2 = Cphid2 * Cthtd2;
+	double Cphid2Sthtd2 = Cphid2 * Sthtd2;
+	double Sphid2Sthtd2 = Sphid2 * Sthtd2;
+	double Sphid2Cthtd2 = Sphid2 * Cthtd2;
+
+	data[0] = Cphid2Cthtd2 * Cpsid2 + Sphid2Sthtd2 * Spsid2;
+	data[1] = Sphid2Cthtd2 * Cpsid2 - Cphid2Sthtd2 * Spsid2;
+	data[2] = Cphid2Sthtd2 * Cpsid2 + Sphid2Cthtd2 * Spsid2;
+	data[3] = Cphid2Cthtd2 * Spsid2 - Sphid2Sthtd2 * Cpsid2;
+
+	Normalize();
+}
+
+FQuaternion FQuaternion::from_axis_angle(FVector v)
+{
+	FQuaternion q;
+	double theta = v.Size();
+	//UE_LOG(LogTemp, Warning, TEXT("Gyro vector size: %f!"), theta);
+	if (theta <= 1e-3)
+	{
+		return q;
+	}
+	v /= theta;
+
+	return from_axis_angle(v, theta);
+}
+
+FQuaternion FQuaternion::from_axis_angle(FVector axis, double theta)
+{
+	if (theta <= 1e-3)
+	{
+		FQuaternion q;
+		return q;
+	}
+	double st2 = sinf(0.5 * theta);
+
+	double q1 = cosf(0.5 * theta);
+	double q2 = axis.X * st2;
+	double q3 = axis.Y * st2;
+	double q4 = axis.Z * st2;
+
+	FQuaternion q(q1, q2, q3, q4);
+	return q;
+}

Source/LuckyWorld/FQuaternion.h

@@ -0,0 +1,36 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+
+/**
+ * 
+ */
+class Matrix33;
+class LUCKYWORLD_API FQuaternion
+{
+public:
+	FQuaternion(FQuat ue_quat);
+	FQuaternion();
+	FQuaternion(double w, double x, double y, double z);
+	~FQuaternion();
+	void Normalize();
+	FQuaternion Inverse();
+	FQuaternion Conjugate();
+	FQuaternion operator*(const FQuaternion& q) const;
+	double& operator()(unsigned int idx);
+	Matrix33 GetMatrix();
+	FVector GetEulerRad();
+	FVector GetEulerDeg();
+	float GetRoll();
+	float GetPitch();
+	float GetYaw();
+	void InitializeFromEulerAngles(double roll, double pitch, double yaw);
+	static FQuaternion from_axis_angle(FVector v);
+	static FQuaternion from_axis_angle(FVector axis, double theta);
+private:
+	double data[4];
+	double GetNorm();
+	float safe_asin(const float v);
+};

Source/LuckyWorld/GPSSensor.cpp

@@ -0,0 +1,111 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "GPSSensor.h"
+
+// Sets default values for this component's properties
+UGPSSensor::UGPSSensor()
+{
+	// Set this component to be initialized when the game starts, and to be ticked every frame.  You can turn these features
+	// off to improve performance if you don't need them.
+	PrimaryComponentTick.bCanEverTick = true;
+
+	// ...
+}
+
+
+// Called when the game starts
+void UGPSSensor::BeginPlay()
+{
+	Super::BeginPlay();
+
+	GTLocation.SetPositionGeodetic(Lon, Lat, Alt);
+	FTransform world_transform = GetComponentTransform();
+	FVector loc = world_transform.GetLocation();
+	FVector ned_loc(loc.X, loc.Y, -loc.Z);
+	Position = ned_loc / 100;
+}
+
+
+// Called every frame
+void UGPSSensor::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
+{
+	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+
+	UpdateGroundTruth(MjStep);
+
+	if (FrameCount % SkipFrames == 0)
+	{
+		UpdateSensor(1/gpsParams.update_rate);
+	}
+
+	FrameCount = (FrameCount + 1) % SkipFrames;
+}
+
+void UGPSSensor::UpdateGroundTruth(float dt)
+{
+	FTransform world_transform = GetComponentTransform();
+	FVector loc = world_transform.GetLocation();
+	FVector ned_loc(loc.X, loc.Y, -loc.Z);
+	ned_loc *= 0.01;
+	if (!InitPosSet)
+	{
+		Position = ned_loc;
+		InitPosSet = true;
+	}
+	FVector lvector = ned_loc - Position;
+	GTLocation.UpdateECFromLocal(lvector);
+	Velocity = lvector / dt;
+	Position = ned_loc;
+}
+
+void UGPSSensor::UpdateSensor(float dt)
+{
+	double XYnoise_stddev = gpsParams.XYNoiseDensity * sqrt(dt);
+	double Znoise_stddev = gpsParams.ZNoiseDensity * sqrt(dt);
+
+	double VXYnoise_stddev = gpsParams.VXYNoiseDensity * sqrt(dt);
+	double VZnoise_stddev = gpsParams.VZNoiseDensity * sqrt(dt);
+
+	double XYRW_stddev = gpsParams.XYRandomWalk * sqrt(dt);
+	double ZRW_stddev = gpsParams.ZRandomWalk * sqrt(dt);
+
+	FVector gps_pos_noise;
+	FVector gps_vel_noise;
+	FVector gps_rw;
+
+	gps_pos_noise.X = RandomGenerator.RandNorm(0, XYnoise_stddev);
+	gps_pos_noise.Y = RandomGenerator.RandNorm(0, XYnoise_stddev);
+	gps_pos_noise.Z = RandomGenerator.RandNorm(0, Znoise_stddev);
+
+	gps_vel_noise.X = RandomGenerator.RandNorm(0, VXYnoise_stddev);
+	gps_vel_noise.Y = RandomGenerator.RandNorm(0, VXYnoise_stddev);
+	gps_vel_noise.Z = RandomGenerator.RandNorm(0, VZnoise_stddev);
+
+	gps_rw.X = RandomGenerator.RandNorm(0, XYRW_stddev);
+	gps_rw.Y = RandomGenerator.RandNorm(0, XYRW_stddev);
+	gps_rw.Z = RandomGenerator.RandNorm(0, ZRW_stddev);
+
+	gpsParams.gps_bias.X += gps_rw.X * dt - gpsParams.gps_bias.X / gpsParams.corellation_time;
+	gpsParams.gps_bias.Y += gps_rw.Y * dt - gpsParams.gps_bias.Y / gpsParams.corellation_time;
+	gpsParams.gps_bias.Z += gps_rw.Z * dt - gpsParams.gps_bias.Z / gpsParams.corellation_time;
+
+	FVector noise_update = gps_pos_noise;
+	Location Wloc(GTLocation);
+	Wloc.UpdateECFromLocal(noise_update);
+	FVector gps_coords = Wloc.GetGeodeticPosition();
+
+	GPSdata.LonRad = gps_coords.X;
+	GPSdata.LatRad = gps_coords.Y;
+	GPSdata.Alt = Position.Z + gps_pos_noise.Z;
+	//GPSdata.Alt = Wloc.GetNEDloc(Wloc).Z;
+
+	GPSdata.VelocityEast = Velocity.Y + gps_vel_noise.Y;
+	GPSdata.VelocityNorth = Velocity.X + gps_vel_noise.X;
+	GPSdata.VelocityUP = Velocity.Z + gps_vel_noise.Z;
+
+
+	double data_array[7] = { GPSdata.LonRad, GPSdata.LatRad, GPSdata.Alt, GPSdata.VelocityEast, GPSdata.VelocityNorth, GPSdata.VelocityUP, dt };
+	DataBuffer.Append(data_array);
+}
+

Source/LuckyWorld/GPSSensor.h

@@ -0,0 +1,86 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "Components/SceneComponent.h"
+#include "Location.h"
+#include "Random.h"
+#include "GPSSensor.generated.h"
+
+USTRUCT()
+struct FGPSData
+{
+	GENERATED_BODY()
+	double LonRad;
+	double LatRad;
+	double Alt;
+	double VelocityEast;
+	double VelocityNorth;
+	double VelocityUP;
+	double dt;
+};
+
+UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) )
+class LUCKYWORLD_API UGPSSensor : public USceneComponent
+{
+	GENERATED_BODY()
+
+public:
+	// Sets default values for this component's properties
+	UGPSSensor();
+
+
+	TArray<double> DataBuffer;
+
+protected:
+	// Called when the game starts
+	virtual void BeginPlay() override;
+
+	struct GPSParams
+	{
+		FVector gps_bias;
+		double update_rate = 5;
+		double XYRandomWalk = 2.0;
+		double ZRandomWalk = 4.0;
+		double XYNoiseDensity = 1.0;
+		double ZNoiseDensity = 1.0;
+		double VXYNoiseDensity = 0.3;
+		double VZNoiseDensity = 0.75;
+		double corellation_time = 60.0;
+	}gpsParams;
+
+	FGPSData GPSdata;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "GPS", meta = (AllowPrivateAccess = true))
+	double Lat = 0;
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "GPS", meta = (AllowPrivateAccess = true))
+	double Lon = 0;
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "GPS", meta = (AllowPrivateAccess = true))
+	double Alt = 0;
+
+	Location GTLocation;
+
+	FVector Position;
+
+	FVector Velocity;
+
+	void UpdateGroundTruth(float dt);
+
+	void UpdateSensor(float dt);
+
+public:
+	// Called every frame
+	virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
+private:
+	Random RandomGenerator;
+
+	//float MjStep = 1 * 0.001667;
+	float MjStep = 1 * 0.001;
+
+	int32 SkipFrames = 1;
+
+	int32 FrameCount = 0;
+
+	bool InitPosSet = false;
+};

Source/LuckyWorld/IMUSensor.cpp

@@ -0,0 +1,217 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "IMUSensor.h"
+#include "Random.h"
+
+// Sets default values for this component's properties
+UIMUSensor::UIMUSensor()
+{
+	// Set this component to be initialized when the game starts, and to be ticked every frame.  You can turn these features
+	// off to improve performance if you don't need them.
+	PrimaryComponentTick.bCanEverTick = true;
+
+	gyroData.MeasurementRange = 2000 * (PI / 180);
+	gyroData.sample_rate = 10;
+	gyroData.Bias = { 0.349, 0.349, 0.349 };
+	//gyroData.Bias = { 0.1, 0.1, 0.1 };
+	gyroData.scale_factor = 0;
+	gyroData.cross_axis_sensitivity = 0;
+	//gyroData.NoiseDencity = 0.01;
+	gyroData.NoiseDencity = 5e-3;
+	//gyroData.RandomWalk = 0.003;
+	//gyroData.RandomWalk = 0.0f;
+	gyroData.BiasCorrelationTime = 1.0e+3;
+	//gyroData.TurnOnBiasSigma = 0.5 / 180.0 * PI;
+	gyroData.TurnOnBiasSigma = 0.0f;
+	gyroData.BiasInstability = 1e-5;
+	double noise_std = 0;
+	double bias_var = 0;
+
+	accelData.MeasurementRange = 16 * GRAVITY;
+	accelData.sample_rate = 10;
+	accelData.Bias = { 0.05, 0.05, 0.08 };
+	accelData.scale_factor = 1;
+	accelData.cross_axis_sensitivity = 1;
+	//accelData.NoiseDencity = 0.00637;;
+	accelData.NoiseDencity = 0.0004;
+	//accelData.RandomWalk = 2.0 * 3.0e-3;
+	accelData.RandomWalk = 0.0f;
+	accelData.BiasCorrelationTime = 300.0;
+	//accelData.TurnOnBiasSigma = 20.0e-3 * 9.8;
+	accelData.TurnOnBiasSigma = 0.0f;
+	accelData.BiasInstability = 0.0;
+
+	magData.sample_rate = 10;
+	magData.NoiseDencity = 5e-6;
+	magData.RandomWalk = 0.0006;
+	magData.BiasCorrelationTime = 6.0e+2;
+	magData.Bias = { 7.5e-07, 7.5e-07, 7.5e-07 };
+	magData.Bias = { 0, 0, 0 };
+	magData.scale_factor = 1.0;
+	magData.BiasInstability = 0;
+
+	Init(gyroData);
+	Init(accelData);
+	Init(magData);
+}
+
+
+// Called when the game starts
+void UIMUSensor::BeginPlay()
+{
+	Super::BeginPlay();
+
+	FTransform world_transform = GetComponentTransform();
+	FVector loc = world_transform.GetLocation();
+	FVector ned_loc(loc.X, loc.Y, -loc.Z);
+	Position = ned_loc / 100;
+	Velocity = { 0, 0, 0 };
+	//InitPosSet = true;
+}
+
+void UIMUSensor::UpdateGroundTruth(float dt)
+{
+	FTransform world_transform = GetComponentTransform();
+	FVector loc = GetComponentLocation();
+	FVector ned_loc(loc.X, loc.Y, -loc.Z);
+	ned_loc *= 0.01f;
+	if (!InitPosSet)
+	{
+		Position = ned_loc;
+		InitPosSet = true;
+	}
+	FVector lvector = ned_loc - Position;
+	FVector vel = lvector / dt;
+	Acceleration = (vel - Velocity) / dt;
+	Velocity = vel;
+	Position = ned_loc;
+
+	FQuat rot = world_transform.GetRotation();
+	FVector euler = rot.Euler();
+	FVector EulerRad = euler * (PI / 180);
+
+	FQuaternion orient;
+	orient.InitializeFromEulerAngles(EulerRad.X, EulerRad.Y, EulerRad.Z);
+	//Tl = orient.GetMatrix();
+	Tl.FromEuler(EulerRad.X, EulerRad.Y, EulerRad.Z);
+	EulerDt = (EulerRad - EulerAngles) / dt;
+	EulerAngles = EulerRad;
+	Tl2b = Tl.Transposed();
+
+	float r = EulerRad.X;
+	float p = EulerRad.Y;
+	float y = EulerRad.Z;
+
+	float srtp = sinf(r) * tanf(p);
+	float crtp = cosf(r) * tanf(p);
+	float srsp = sinf(r) / cosf(p);
+	float crsp = cosf(r) / cosf(p);
+
+	Matrix33 R(1, srtp, crtp, 0, cosf(p), -sinf(p), 0, srsp, crsp);
+
+	PQR = R.Inverse() * EulerDt;
+
+	/*
+	double s = 2 / dt;
+	double wx = s * (Orient(0) * orient(1) - Orient(1) * orient(0) - Orient(2) * orient(3) + Orient(3) * orient(2));
+	double wy = s * (Orient(0) * orient(2) + Orient(1) * orient(3) - Orient(2) * orient(0) - Orient(3) * orient(1));
+	double wz = s * (Orient(0) * orient(3) - Orient(1) * orient(2) + Orient(2) * orient(1) - Orient(3) * orient(0));
+
+	PQR.X = wx;
+	PQR.Y = wy;
+	PQR.Z = wz;
+	//PQR = Tl2b * PQR;
+	Orient = orient;
+	*/
+
+	FVector gravity(0, 0, -GRAVITY);
+	Acceleration = Tl2b * (Acceleration + gravity);
+
+
+	FVector mag_ef(MF_INTENCITY, 0, 0);
+	FQuaternion quat;
+	quat.InitializeFromEulerAngles(0, -MF_INCLINATION, MF_DECLINATION);
+	//Matrix33 mat = quat.GetMatrix();
+	Matrix33 mat;
+	mat.FromEuler(0.0, -MF_INCLINATION, MF_DECLINATION);
+	mag_ef = mat * mag_ef;
+	MagneticField = Tl2b * mag_ef;
+}
+
+
+// Called every frame
+void UIMUSensor::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
+{
+	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+
+	UpdateGroundTruth(MjStep);
+
+	for (int i = 0; i < SubSteps; i++)
+	{
+		UpdateSensors(gyroData, PQR, recentGyro, MjStep);
+		UpdateSensors(accelData, Acceleration, recentAccel, MjStep);
+		UpdateSensors(magData, MagneticField, recentMag, MjStep);
+
+		WriteBuffers();
+	}
+}
+
+void  UIMUSensor::Init(IMUParams& params)
+{
+	double dt = 1 / params.sample_rate;
+	params.bias_var = ((2 * pow(params.BiasInstability, 2)) / PI) * log(2);
+	params.noise_std = params.NoiseDencity * sqrt(params.sample_rate);
+	Matrix33 identity;
+	double scaleX = RandomGenerator.UniformRand(-params.scale_factor, params.scale_factor) / 100.0;
+	double scaleY = RandomGenerator.UniformRand(-params.scale_factor, params.scale_factor) / 100.0;
+	double scaleZ = RandomGenerator.UniformRand(-params.scale_factor, params.scale_factor) / 100.0;
+	Matrix33 scale_matrix(scaleX, 0, 0, 0, scaleY, 0, 0, 0, scaleZ);
+	//params.AxesScaleAndCC = identity + scale_matrix;
+	params.AxesScaleAndCC = identity;
+	double ca1 = RandomGenerator.UniformRand(-params.cross_axis_sensitivity, params.cross_axis_sensitivity) / 100.0;
+	double ca2 = RandomGenerator.UniformRand(-params.cross_axis_sensitivity, params.cross_axis_sensitivity) / 100.0;
+	double ca3 = RandomGenerator.UniformRand(-params.cross_axis_sensitivity, params.cross_axis_sensitivity) / 100.0;
+	Matrix33 axes_misalign(0, ca1, ca2, ca1, 0, ca3, ca2, ca3, 0);
+	params.AxesScaleAndCC = params.AxesScaleAndCC + axes_misalign;
+}
+
+void UIMUSensor::UpdateSensors(IMUParams& params, FVector true_m, FIMUOutputData& dataStruct, double dt)
+{
+	/*
+	double tau = params.BiasCorrelationTime;
+	double sigma_d = 1 / sqrt(dt) * params.NoiseDencity;
+	double sigma_b = params.RandomWalk;
+	double sigma_b_d = sqrt(-sigma_b * sigma_b * tau / 2.0 * (exp(-2.0 * dt / tau) - 1.0));
+	double phi_d = exp(-1.0 / tau * dt);
+
+	params.Bias.X = phi_d * params.Bias.X + RandomGenerator.RandNorm(0, sigma_b_d);
+	dataStruct.data.X = true_m.X + params.Bias.X + RandomGenerator.RandNorm(0, sigma_d);
+
+	params.Bias.Y = phi_d * params.Bias.Y + RandomGenerator.RandNorm(0, sigma_b_d);
+	dataStruct.data.Y = true_m.Y + params.Bias.Y + RandomGenerator.RandNorm(0, sigma_d);
+
+	params.Bias.Z = phi_d * params.Bias.Z + RandomGenerator.RandNorm(0, sigma_b_d);
+	dataStruct.data.Z = true_m.Z + params.Bias.Z + RandomGenerator.RandNorm(0, sigma_d);
+
+	dataStruct.dt = dt;
+	*/
+
+	//double noise_std = params.NoiseDencity * sqrtf(params.sample_rate);
+	params.Bias = params.Bias + dt * RandomGenerator.RandNorm(0, sqrtf(params.bias_var));
+	dataStruct.data = params.Bias + params.AxesScaleAndCC * true_m + RandomGenerator.RandNorm(0, params.noise_std);
+	dataStruct.dt = dt;
+}
+
+void UIMUSensor::WriteBuffers()
+{
+	double gyro[4] = { recentGyro.data.X,  recentGyro.data.Y,  recentGyro.data.Z, recentGyro.dt };
+	GyroDataBuffer.Append(gyro, 4);
+
+	double accel[4] = { recentAccel.data.X, recentAccel.data.Y, recentAccel.data.Z, recentAccel.dt };
+	AccelDataBuffer.Append(accel, 4);
+
+	double mag[4] = { recentMag.data.X, recentMag.data.Y, recentMag.data.Z, recentMag.dt };
+	MagDataBuffer.Append(mag, 4);
+}
+

Source/LuckyWorld/IMUSensor.h

@@ -0,0 +1,128 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "Components/SceneComponent.h"
+#include "Matrix33.h"
+#include "FQuaternion.h"
+#include "Random.h"
+#include "IMUSensor.generated.h"
+
+#ifndef GRAVITY
+#define GRAVITY      9.81
+#endif
+
+#ifndef MF_INCLINATION
+#define  MF_INCLINATION      -30.89 * (PI/180)
+#endif
+
+#ifndef MF_DECLINATION
+#define  MF_DECLINATION      -4.016 * (PI/180)
+#endif
+
+#ifndef MF_INTENCITY
+#define  MF_INTENCITY        0.3184
+#endif
+
+USTRUCT()
+struct FIMUOutputData
+{
+	GENERATED_BODY()
+	FVector data;
+	float dt;
+};
+
+
+UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) )
+class LUCKYWORLD_API UIMUSensor : public USceneComponent
+{
+	GENERATED_BODY()
+
+public:	
+	// Sets default values for this component's properties
+	UIMUSensor();
+
+	TArray<double> GyroDataBuffer;
+
+	TArray<double> AccelDataBuffer;
+
+	TArray<double> MagDataBuffer;
+
+protected:
+	// Called when the game starts
+	virtual void BeginPlay() override;
+
+	struct IMUParams
+	{
+		double MeasurementRange = 0;
+		uint16 sample_rate = 10;
+		Matrix33 AxesScaleAndCC;
+		FVector Bias;
+		double scale_factor = 0;
+		double cross_axis_sensitivity = 0;
+		double ARWcoef = 0;
+		double NoiseDencity = 0;
+		double BiasInstability = 0;
+		double RandomWalk = 0;
+		double TurnOnBiasSigma = 0;
+		double BiasCorrelationTime = 0;
+		double noise_std = 0;
+		double bias_var = 0;
+	};
+
+	IMUParams gyroData;
+
+	FIMUOutputData recentGyro;
+
+	IMUParams accelData;
+
+	FIMUOutputData recentAccel;
+
+	IMUParams magData;
+
+	FIMUOutputData recentMag;
+
+	void Init(IMUParams& params);
+
+	Random RandomGenerator;
+
+	void UpdateGroundTruth(float dt);
+
+	void UpdateSensors(IMUParams& params, FVector true_m, FIMUOutputData& dataStruct, double dt);
+
+	void WriteBuffers();
+
+public:	
+	// Called every frame
+	virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
+
+private:
+	//float MjStep = 0.001667;
+	float MjStep = 0.001;
+
+	int32 SubSteps = 1;
+
+	int32 WarmUpCounter = 3;
+
+	bool InitPosSet = false;
+
+	FVector EulerAngles;
+
+	FVector EulerDt;
+
+	FVector PQR;
+
+	FQuaternion Orient;
+
+	Matrix33 Tl;
+	Matrix33 Tl2b;
+
+	FVector Position;
+
+	FVector Velocity;
+
+	FVector Acceleration;
+
+	FVector MagneticField;
+};

Source/LuckyWorld/Location.cpp

@@ -0,0 +1,167 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Location.h"
+
+Location::Location()
+{
+	//UE_LOG(LogTemp, Display, TEXT("New Location"));
+	ec = Earth_b / Earth_a;
+	ec2 = ec * ec;
+	e2 = 1.0 - ec2;
+	c = Earth_a * e2;
+	//UE_LOG(LogTemp, Display, TEXT("New Location vars: %f %f %f %f"), ec, ec2, e2, c);
+}
+
+Location::Location(const Location& l)
+{
+	e2 = l.e2;
+	c = l.c;
+	ec = l.ec;
+	ec2 = l.ec2;
+
+	ECLoc = l.ECLoc;
+
+	Lon = l.Lon;
+	Lat = l.Lat;
+	mRadius = l.mRadius;
+
+	Tl2ec = l.Tl2ec;
+	Tec2l = l.Tec2l;
+
+	mGeodLat = l.mGeodLat;
+	mGeodAlt = l.mGeodAlt;
+}
+
+Location::~Location()
+{
+}
+
+void  Location::SetPositionGeodetic(double lon, double lat, double height)
+{
+	double slat = sin(lat);
+	double clat = cos(lat);
+	double RN = Earth_a / sqrt(1.0 - e2 * slat * slat);
+
+	//UE_LOG(LogTemp, Display, TEXT("Geodetic vars: %f %f %f"), slat, clat, RN);
+
+	ECLoc.X = (RN + height) * clat * cos(lon);
+	ECLoc.Y = (RN + height) * clat * sin(lon);
+	ECLoc.Z = ((1 - e2) * RN + height) * slat;
+
+	ComputeDerived();
+}
+
+FVector Location::GetGeodeticPosition()
+{
+	FVector geodetic(Lon, Lat, mGeodAlt);
+	return geodetic;
+}
+
+void Location::ComputeDerived()
+{
+	//UE_LOG(LogTemp, Display, TEXT("EC loc %f %f %f"), ECLoc.X, ECLoc.Y, ECLoc.Z)
+	mRadius = ECLoc.Size();
+	//UE_LOG(LogTemp, Display, TEXT("mRadius %f"), mRadius)
+	double rxy = ECLoc.Size2D();
+	//UE_LOG(LogTemp, Display, TEXT("rxy %f"), rxy)
+
+	double sinLon, cosLon;
+	if (rxy == 0.0)
+	{
+		sinLon = 0.0;
+		cosLon = 1.0;
+		Lon = 0.0;
+	}
+	else
+	{
+		sinLon = ECLoc.Y / rxy;
+		cosLon = ECLoc.X / rxy;
+		Lon = atan2(ECLoc.Y, ECLoc.X);
+	}
+
+	double sinLat, cosLat;
+	if (mRadius == 0.0)
+	{
+		Lat = 0.0;
+		sinLat = 0.0;
+		cosLat = 1.0;
+		mGeodLat = 0.0;
+		mGeodAlt = -Earth_a;
+	}
+	else
+	{
+		Lat = atan2(ECLoc.Z, rxy);
+		double s0 = fabs(ECLoc.Z);
+		double zc = ec * s0;
+		double c0 = ec * rxy;
+		double c02 = c0 * c0;
+		double s02 = s0 * s0;
+		double a02 = c02 + s02;
+		double a0 = sqrt(a02);
+		double a03 = a02 * a0;
+		double s1 = zc * a03 + c * s02 * s0;
+		double c1 = rxy * a03 - c * c02 * c0;
+		double cs0c0 = c * c0 * s0;
+		double b0 = 1.5 * cs0c0 * ((rxy * s0 - zc * c0) * a0 - cs0c0);
+		s1 = s1 * a03 - b0 * s0;
+		double cc = ec * (c1 * a03 - b0 * c0);
+		mGeodLat = sign(ECLoc.Z) * atan(s1 / cc);
+		double s12 = s1 * s1;
+		double cc2 = cc * cc;
+		double norm = sqrt(s12 + cc2);
+		cosLat = cc / norm;
+		sinLat = sign(ECLoc.Z) * s1 / norm;
+		mGeodAlt = (rxy * cc + s0 * s1 - Earth_a * sqrt(ec2 * s12 + cc2)) / norm;
+	}
+	Matrix33 m(-cosLon * sinLat, -sinLon * sinLat, cosLat, -sinLon, cosLon, 0.0, -cosLon * cosLat, -sinLon * cosLat, -sinLat);
+	//UE_LOG(LogTemp, Display, TEXT("Matrix vector: %f, %f, %f, %f, %f, %f, %f, %f, %f"), -cosLon * sinLat, -sinLon * sinLat, cosLat, -sinLon, cosLon, 0.0, -cosLon * cosLat, -sinLon * cosLat, -sinLat)
+	Tec2l = m;
+	Tl2ec = Tec2l.Transposed();
+}
+
+void Location::UpdateECFromLocal(FVector lvector)
+{
+	ECLoc = (Tl2ec * lvector) + ECLoc;
+	ComputeDerived();
+}
+
+void Location::UpdateEC(FVector update)
+{
+	ECLoc += update;
+	ComputeDerived();
+}
+
+Location& Location::operator=(const Location& l)
+{
+	ECLoc = l.ECLoc;
+
+	Earth_a = l.Earth_a;
+	e2 = l.e2;
+	c = l.c;
+	ec = l.ec;
+	ec2 = l.ec2;
+
+	Lon = l.Lon;
+	Lat = l.Lat;
+	mRadius = l.mRadius;
+
+	Tl2ec = l.Tl2ec;
+	Tec2l = l.Tec2l;
+
+	mGeodLat = l.mGeodLat;
+	mGeodAlt = l.mGeodAlt;
+
+	return *this;
+}
+
+FVector Location::GetDistanceNED(const Location& loc) const
+{
+	//UE_LOG(LogTemp, Display, TEXT("This EC vector: %f, %f, %f"), ECLoc.X, ECLoc.Y, ECLoc.Z);
+	//UE_LOG(LogTemp, Display, TEXT("Loc EC vector: %f, %f, %f"), loc.ECLoc.X, loc.ECLoc.Y, loc.ECLoc.Z);
+	FVector dst = loc.ECLoc - ECLoc;
+	//UE_LOG(LogTemp, Display, TEXT("EC vector: %f, %f, %f"), dst.X, dst.Y, dst.Z)
+	FVector ned_dst = Tec2l * dst;
+	//UE_LOG(LogTemp, Display, TEXT("NED vector: %f, %f, %f"), ned_dst.X, ned_dst.Y, ned_dst.Z)
+	return ned_dst;
+}

Source/LuckyWorld/Location.h

@@ -0,0 +1,45 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "Matrix33.h"
+
+/**
+ * 
+ */
+class LUCKYWORLD_API Location
+{
+public:
+	Location();
+	Location(const Location& l);
+	~Location();
+	void UpdateECFromLocal(FVector lvector);
+	void UpdateEC(FVector update);
+	FVector GetECLoc() { return ECLoc; }
+	void SetPositionGeodetic(double lon, double lat, double height);
+	FVector GetGeodeticPosition();
+	FVector GetDistanceNED(const Location& loc) const;
+	FVector GetNEDloc(const Location& loc) const { return Tec2l * ECLoc + GetDistanceNED(loc); };
+	double GetAlt() { return mGeodAlt; };
+	Location& operator=(const Location& l);
+private:
+	double sign(double num) { return num >= 0.0 ? 1.0 : -1.0; }
+	FVector ECLoc;
+	double Lon;
+	double Lat;
+	double mRadius;
+	double mGeodLat;
+	double mGeodAlt;
+	Matrix33 Tec2l;
+	Matrix33 Tl2ec;
+
+	double Earth_a = 6378137.0;
+	double Earth_b = 6356752.314245;
+	double e2;
+	double c;
+	double ec;
+	double ec2;
+
+	void ComputeDerived();
+};

Source/LuckyWorld/Matrix33.cpp

@@ -0,0 +1,299 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Matrix33.h"
+#include "FQuaternion.h"
+
+Matrix33::Matrix33()
+{
+	data[0] = 1;
+	data[1] = 0;
+	data[2] = 0;
+	data[3] = 0;
+	data[4] = 1;
+	data[5] = 0;
+	data[6] = 0;
+	data[7] = 0;
+	data[8] = 1;
+}
+
+Matrix33::Matrix33(const double m11, const double m12, const double m13,
+	const double m21, const double m22, const double m23,
+	const double m31, const double m32, const double m33)
+{
+	data[0] = m11;
+	data[1] = m21;
+	data[2] = m31;
+	data[3] = m12;
+	data[4] = m22;
+	data[5] = m32;
+	data[6] = m13;
+	data[7] = m23;
+	data[8] = m33;
+}
+
+void Matrix33::FromEuler(float roll, float pitch, float yaw)
+{
+	float cp = cosf(pitch);
+	float sp = sinf(pitch);
+	float sr = sinf(roll);
+	float cr = cosf(roll);
+	float sy = sinf(yaw);
+	float cy = cosf(yaw);
+
+	data[0] = cp * cy;
+	data[3] = (sr * sp * cy) - (cr * sy);
+	data[6] = (cr * sp * cy) + (sr * sy);
+	data[1] = cp * sy;
+	data[4] = (sr * sp * sy) + (cr * cy);
+	data[7] = (cr * sp * sy) - (sr * cy);
+	data[2] = -sp;
+	data[5] = sr * cp;
+	data[8] = cr * cp;
+}
+
+FVector Matrix33::A() const
+{
+	return FVector(data[0], data[3], data[6]);
+}
+
+FVector Matrix33::B() const
+{
+	return FVector(data[1], data[4], data[7]);
+}
+
+FVector Matrix33::C() const
+{
+	return FVector(data[2], data[5], data[8]);
+}
+
+void Matrix33::SetRow(FVector row, int row_idx)
+{
+	if (row_idx == a)
+	{
+		data[0] = row.X;
+		data[3] = row.Y;
+		data[6] = row.Z;
+	}
+	if (row_idx == b)
+	{
+		data[1] = row.X;
+		data[4] = row.Y;
+		data[7] = row.Z;
+	}
+	if (row_idx == c)
+	{
+		data[2] = row.X;
+		data[5] = row.Y;
+		data[8] = row.Z;
+	}
+}
+
+Matrix33 Matrix33::Transposed()
+{
+	return Matrix33(data[0], data[1], data[2],
+		data[3], data[4], data[5],
+		data[6], data[7], data[8]);
+}
+
+Matrix33::~Matrix33()
+{
+}
+
+FVector Matrix33::GetAxis(int idx)
+{
+	if (idx == X)
+		return FVector(data[0], data[1], data[2]);
+	if (idx == Y)
+		return FVector(data[3], data[4], data[5]);
+	return FVector(data[6], data[7], data[8]);
+}
+
+Matrix33 Matrix33::GetRotationMatrix(double roll, double pitch, double yaw) const
+{
+	Matrix33 r_z = Matrix33(cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1);
+	Matrix33 r_y = Matrix33(cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch));
+	Matrix33 r_x = Matrix33(1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll));
+
+	//Matrix33 res = r_z * r_y * r_x;
+	Matrix33 res = r_x * r_y * r_z;
+	//Matrix33 res = r_y * r_z * r_x;
+	return res;
+}
+
+Matrix33 Matrix33::operator+(const Matrix33& M) const
+{
+	return Matrix33(data[0] + M.data[0],
+		data[3] + M.data[3],
+		data[6] + M.data[6],
+		data[1] + M.data[1],
+		data[4] + M.data[4],
+		data[7] + M.data[7],
+		data[2] + M.data[2],
+		data[5] + M.data[5],
+		data[8] + M.data[8]);
+}
+
+Matrix33 Matrix33::operator*(const double scalar) const
+{
+	return Matrix33(scalar * data[0],
+		scalar * data[3],
+		scalar * data[6],
+		scalar * data[1],
+		scalar * data[4],
+		scalar * data[7],
+		scalar * data[2],
+		scalar * data[5],
+		scalar * data[8]);
+}
+
+Matrix33 Matrix33::operator*(const Matrix33& M) const
+{
+	Matrix33 Product;
+
+	Product.data[0] = data[0] * M.data[0] + data[3] * M.data[1] + data[6] * M.data[2];
+	Product.data[3] = data[0] * M.data[3] + data[3] * M.data[4] + data[6] * M.data[5];
+	Product.data[6] = data[0] * M.data[6] + data[3] * M.data[7] + data[6] * M.data[8];
+	Product.data[1] = data[1] * M.data[0] + data[4] * M.data[1] + data[7] * M.data[2];
+	Product.data[4] = data[1] * M.data[3] + data[4] * M.data[4] + data[7] * M.data[5];
+	Product.data[7] = data[1] * M.data[6] + data[4] * M.data[7] + data[7] * M.data[8];
+	Product.data[2] = data[2] * M.data[0] + data[5] * M.data[1] + data[8] * M.data[2];
+	Product.data[5] = data[2] * M.data[3] + data[5] * M.data[4] + data[8] * M.data[5];
+	Product.data[8] = data[2] * M.data[6] + data[5] * M.data[7] + data[8] * M.data[8];
+
+	return Product;
+}
+
+FQuaternion Matrix33::GetQuaternion(void) const
+{
+	FQuaternion Q;
+
+	double tempQ[4];
+	int idx;
+
+	tempQ[0] = 1.0 + data[0] + data[4] + data[8];
+	tempQ[1] = 1.0 + data[0] - data[4] - data[8];
+	tempQ[2] = 1.0 - data[0] + data[4] - data[8];
+	tempQ[3] = 1.0 - data[0] - data[4] + data[8];
+
+	// Find largest of the above
+	idx = 0;
+	for (int i = 1; i < 4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
+
+	switch (idx) {
+	case 0:
+		Q(0) = 0.50 * sqrt(tempQ[0]);
+		Q(1) = 0.25 * (data[7] - data[5]) / Q(0);
+		Q(2) = 0.25 * (data[2] - data[6]) / Q(0);
+		Q(3) = 0.25 * (data[3] - data[1]) / Q(0);
+		break;
+	case 1:
+		Q(1) = 0.50 * sqrt(tempQ[1]);
+		Q(0) = 0.25 * (data[7] - data[5]) / Q(1);
+		Q(2) = 0.25 * (data[3] + data[1]) / Q(1);
+		Q(3) = 0.25 * (data[2] + data[6]) / Q(1);
+		break;
+	case 2:
+		Q(2) = 0.50 * sqrt(tempQ[2]);
+		Q(0) = 0.25 * (data[2] - data[6]) / Q(2);
+		Q(1) = 0.25 * (data[3] + data[1]) / Q(2);
+		Q(3) = 0.25 * (data[7] + data[5]) / Q(2);
+		break;
+	case 3:
+		Q(3) = 0.50 * sqrt(tempQ[3]);
+		Q(0) = 0.25 * (data[3] - data[1]) / Q(3);
+		Q(1) = 0.25 * (data[6] + data[2]) / Q(3);
+		Q(2) = 0.25 * (data[7] + data[5]) / Q(3);
+		break;
+	default:
+		//error
+		break;
+	}
+
+	return Q;
+}
+
+FVector Matrix33::GetEuler(void) const
+{
+	FVector mEulerAngles;
+	bool GimbalLock = false;
+
+	if (data[6] <= -1.0)
+	{
+		mEulerAngles.Y = 0.5 * PI;
+		GimbalLock = true;
+	}
+	else if (1.0 <= data[6]) {
+		mEulerAngles.Y = -0.5 * PI;
+		GimbalLock = true;
+	}
+	else
+		mEulerAngles.Y = asin(-data[6]);
+
+	if (GimbalLock)
+		mEulerAngles.X = atan2(-data[5], data[4]);
+	else
+		mEulerAngles.X = atan2(data[7], data[8]);
+
+	if (GimbalLock)
+		mEulerAngles.Z = 0.0;
+	else {
+		double psi = atan2(data[3], data[0]);
+		if (psi < 0.0)
+			psi += 2 * PI;
+		mEulerAngles.Z = psi;
+	}
+
+	return mEulerAngles;
+}
+
+double Matrix33::Determinant(void) const
+{
+	return data[0] * data[4] * data[8] + data[3] * data[7] * data[2]
+		+ data[6] * data[1] * data[5] - data[6] * data[4] * data[2]
+		- data[3] * data[1] * data[8] - data[7] * data[5] * data[0];
+}
+
+Matrix33 Matrix33::Inverse(void) const
+{
+	if (Determinant() != 0.0)
+	{
+		double rdet = 1.0 / Determinant();
+
+		double i11 = rdet * (data[4] * data[8] - data[7] * data[5]);
+		double i21 = rdet * (data[7] * data[2] - data[1] * data[8]);
+		double i31 = rdet * (data[1] * data[5] - data[4] * data[2]);
+		double i12 = rdet * (data[6] * data[5] - data[3] * data[8]);
+		double i22 = rdet * (data[0] * data[8] - data[6] * data[2]);
+		double i32 = rdet * (data[3] * data[2] - data[0] * data[5]);
+		double i13 = rdet * (data[3] * data[7] - data[6] * data[4]);
+		double i23 = rdet * (data[6] * data[1] - data[0] * data[7]);
+		double i33 = rdet * (data[0] * data[4] - data[3] * data[1]);
+
+		return Matrix33(i11, i12, i13, i21, i22, i23, i31, i32, i33);
+	}
+	else {
+		return Matrix33(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	}
+}
+
+FVector Matrix33::operator*(const FVector& v) const
+{
+	double v1 = v.X;
+	double v2 = v.Y;
+	double v3 = v.Z;
+
+	double tmp1 = v1 * data[0];
+	double tmp2 = v1 * data[1];
+	double tmp3 = v1 * data[2];
+
+	tmp1 += v2 * data[3];
+	tmp2 += v2 * data[4];
+	tmp3 += v2 * data[5];
+
+	tmp1 += v3 * data[6];
+	tmp2 += v3 * data[7];
+	tmp3 += v3 * data[8];
+
+	return FVector(tmp1, tmp2, tmp3);
+}

Source/LuckyWorld/Matrix33.h

@@ -0,0 +1,56 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+
+/**
+ * 
+ */
+class FQuaternion;
+class LUCKYWORLD_API Matrix33
+{
+public:
+	enum { X = 1, Y, Z };
+	enum { a = 1, b, c };
+	Matrix33();
+	Matrix33(const double m11, const double m12, const double m13,
+		const double m21, const double m22, const double m23,
+		const double m31, const double m32, const double m33);
+	~Matrix33();
+	FVector A() const;
+	FVector B() const;
+	FVector C() const;
+	void SetRow(FVector row, int row_idx);
+	Matrix33 Transposed();
+	FQuaternion GetQuaternion() const;
+	FVector GetEuler() const;
+	Matrix33 Inverse() const;
+	void FromEuler(float roll, float pitch, float yaw);
+	FVector operator * (const FVector& v) const;
+	Matrix33 operator * (const Matrix33& B) const;
+	Matrix33 operator+(const Matrix33& M) const;
+	Matrix33 operator*(const double scalar) const;
+	FVector GetAxis(int idx);
+	Matrix33 GetRotationMatrix(double roll, double pitch, double yaw) const;
+	Matrix33& operator=(const Matrix33& A)
+	{
+		data[0] = A.data[0];
+		data[1] = A.data[1];
+		data[2] = A.data[2];
+		data[3] = A.data[3];
+		data[4] = A.data[4];
+		data[5] = A.data[5];
+		data[6] = A.data[6];
+		data[7] = A.data[7];
+		data[8] = A.data[8];
+		return *this;
+	}
+	double operator()(unsigned int row, unsigned int col) const
+	{
+		return data[row * 3 + col];
+	}
+private:
+	double data[9];
+	double Determinant() const;
+};

Source/LuckyWorld/MultiRotorDrone.cpp

@@ -0,0 +1,943 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "MultiRotorDrone.h"
+#include "Misc/DateTime.h"
+#include <stdlib.h>
+#include <math.h>
+#include "Kismet/GameplayStatics.h"
+
+// Sets default values
+AMultiRotorDrone::AMultiRotorDrone()
+{
+	// Set this pawn to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+	PrimaryActorTick.bCanEverTick = true;
+
+}
+
+// Called when the game starts or when spawned
+void AMultiRotorDrone::BeginPlay()
+{
+	Super::BeginPlay();
+	Kt = AIR_DENCITY * Rotors.Ct * powf(Rotors.Diameter, 4);
+	Km = AIR_DENCITY * Rotors.Cq * powf(Rotors.Diameter, 5);
+	minThrust = 0.5 * Mass * GRAVITY;
+
+	maxThrustPerMotor = ((MaxRPM / 60) * (MaxRPM / 60)) * Kt;
+
+	MaxThrust = maxThrustPerMotor * 4;
+
+	SrefZ = sqrtf(Rotors.p1.X * Rotors.p1.X + Rotors.p1.Y * Rotors.p1.Y) * sqrtf(Rotors.p2.X * Rotors.p2.X + Rotors.p2.Y * Rotors.p2.Y);
+
+	COMLocation = { -0.02, 0.0, -0.29 };
+
+	if (Mode == GUIDED)
+	{
+		AltitudeManualControl = false;
+		XYmanualControl = false;
+	}
+}
+
+void AMultiRotorDrone::SetTrueState(FTrueDroneState true_state)
+{
+	FVector prev_position = TrueState.Position;
+	FVector prev_euler = TrueState.Orientation;
+	FVector prev_velocity = TrueState.Velocity;
+	TrueState.Position = true_state.Position * 0.01;
+	TrueState.Position.Z *= -1.0;
+	TrueState.Orientation = true_state.Orientation * (PI / 180);
+	if (!TrueState.initialised)
+	{
+		if (use_true_state)
+		{
+			TargetPosition = TrueState.Position;
+			TargetYaw = TrueState.Orientation.Z;
+		}
+		TrueState.initialised = true;
+		return;
+	}
+	//UE_LOG(LogTemp, Warning, TEXT("Prev position %f %f %f"), prev_position.X, prev_position.Y, prev_position.Z)
+	//UE_LOG(LogTemp, Warning, TEXT("Curr position %f %f %f"), TrueState.Position.X, TrueState.Position.Y, TrueState.Position.Z)
+	TrueState.Velocity = (TrueState.Position - prev_position) / MjStep;
+	TrueState.Acceleration = (TrueState.Velocity - prev_velocity) / MjStep;
+	FVector EulerDt = (TrueState.Orientation - prev_euler) / MjStep;
+	EulerDt2PQR(EulerDt);
+}
+
+void AMultiRotorDrone::SetCurrentState()
+{
+	FVector prev_position = State.Position;
+	FVector prev_euler = State.Orientation;
+	FVector prev_velocity = State.Velocity;
+	if (use_true_state)
+	{
+		State = TrueState;
+		return;
+	}
+	State.Position = EstState.position;
+	State.Orientation = EstState.quat.GetEulerRad();
+	State.Velocity = EstState.velocity;
+	//UE_LOG(LogTemp, Display, TEXT("True orientation degrees %f %f %f"), true_state.Orientation.X, true_state.Orientation.Y, true_state.Orientation.Z);
+	if (!State.initialised)
+	{
+		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);
+}
+
+void AMultiRotorDrone::EulerDt2PQR(FVector& EulerDt)
+{
+	float r = State.Orientation.X;
+	float p = State.Orientation.Y;
+	float y = State.Orientation.Z;
+
+	float srtp = sinf(r) * tanf(p);
+	float crtp = cosf(r) * tanf(p);
+	float srsp = sinf(r) / cosf(p);
+	float crsp = cosf(r) / cosf(p);
+
+	Matrix33 R(1, srtp, crtp, 0, cosf(p), -sinf(p), 0, srsp, crsp);
+
+	PQR = R.Inverse() * EulerDt;
+}
+
+void AMultiRotorDrone::SetImuSensor(UIMUSensor* sensor)
+{
+	Imu_sensor = sensor;
+	StateEstimator.RegisterBuffer(Sensor::GYRO, Imu_sensor->GyroDataBuffer);
+	StateEstimator.RegisterBuffer(Sensor::ACCEL, Imu_sensor->AccelDataBuffer);
+	StateEstimator.RegisterBuffer(Sensor::MAG, Imu_sensor->MagDataBuffer);
+}
+
+void  AMultiRotorDrone::SetGPSSensor(UGPSSensor* sensor)
+{
+	GPS_sensor = sensor;
+	StateEstimator.RegisterBuffer(Sensor::GPS, GPS_sensor->DataBuffer);
+}
+
+void AMultiRotorDrone::SetBaroSensor(UBarometer* sensor)
+{
+	Baro_sensor = sensor;
+	StateEstimator.RegisterBuffer(Sensor::BARO, Baro_sensor->DataBuffer);
+}
+
+// Called every frame
+void AMultiRotorDrone::Tick(float DeltaTime)
+{
+	Super::Tick(DeltaTime);
+	//ProcessIMUData();
+
+	if (!StateEstimator.IsInitialised())
+	{
+		StateEstimator.InitialiseFilter();
+	}
+	else
+	{
+		StateEstimator.Update();
+		StateEstimator.GetState(EstState);
+		SetCurrentState();
+		ControllerUpdate();
+	}
+
+	UE_LOG(LogTemp, Warning, TEXT("EKF estimated position: %f, %f, %f!"), EstState.position.X, EstState.position.Y, EstState.position.Z);
+	UE_LOG(LogTemp, Warning, TEXT("EKF estimated velocity: %f, %f, %f!"), EstState.velocity.X, EstState.velocity.Y, EstState.velocity.Z);
+	FVector Euler = EstState.quat.GetEulerDeg();
+	UE_LOG(LogTemp, Warning, TEXT("EKF estimated orientation: %f, %f, %f!"), Euler.X, Euler.Y, Euler.Z);
+
+	UE_LOG(LogTemp, Warning, TEXT("True position: %f, %f, %f!"), TrueState.Position.X, TrueState.Position.Y, TrueState.Position.Z);
+	UE_LOG(LogTemp, Warning, TEXT("True velocity: %f, %f, %f!"), TrueState.Velocity.X, TrueState.Velocity.Y, TrueState.Velocity.Z);
+	UE_LOG(LogTemp, Warning, TEXT("True orientation: %f, %f, %f!"), TrueState.Orientation.X, TrueState.Orientation.Y, TrueState.Orientation.Z);
+}
+
+// Called to bind functionality to input
+void AMultiRotorDrone::SetupPlayerInputComponent(UInputComponent* PlayerInputComponent)
+{
+	Super::SetupPlayerInputComponent(PlayerInputComponent);
+	//PlayerInputComponent->BindAxis(TEXT("height_increase"), this, &AMultiRotorDrone::IncreaseTargetHeight);
+}
+
+void AMultiRotorDrone::IncreaseDecreaseTargetHeight(float value)
+{
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+	if (Mode == GUIDED)
+	{
+		LandingStarted = false;
+		TargetPosition.Z -= MaxVelocity * dt * value;
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(TargetPosition.Z));
+	}
+	else if (Mode == MANUAL)
+	{
+		if(KeepAltitude) KeepAltitude = false;
+		if (!AltitudeManualControl) AltitudeManualControl = true;
+		CurrentThrottle += ThrottleRate * dt * value;
+		if (CurrentThrottle > PWM_max_us)
+		{
+			CurrentThrottle = PWM_max_us;
+		}
+
+		if (CurrentThrottle < PWM_min_us)
+		{
+			CurrentThrottle = PWM_min_us;
+		}
+
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(CurrentThrottle));
+	}
+	//UE_LOG(LogTemp, Display, TEXT("Current Height Desired: %f %f"), TargetPosition.Z, value);
+}
+
+void AMultiRotorDrone::HoldAltitude()
+{
+	//TargetPosition.Z = State.Position.Z;
+	//TargetPosition.X = State.Position.X;
+	//TargetPosition.Y = State.Position.Y;
+	KeepAltitude = true;
+	//KeepXYPosition = true;
+	CurrentThrottle = 1500;
+	pid.Xvel_int = 0.0;
+	pid.Yvel_int = 0.0;
+}
+
+void AMultiRotorDrone::MoveLeftRight(float value)
+{
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+	LandingStarted = false;
+	if (Mode == GUIDED)
+	{
+		TargetPosition.Y += MaxVelocity * dt * value;
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(TargetPosition.Y));
+	}
+	else if (Mode == MANUAL)
+	{
+		if (KeepXYPosition) KeepXYPosition = false;
+		if (!XYmanualControl) XYmanualControl = true;
+		CurrentRollPWM += RollPitchPWMRate * dt * value;
+		if (CurrentRollPWM > PWM_max_us)
+		{
+			CurrentRollPWM = PWM_max_us;
+		}
+
+		if (CurrentRollPWM < PWM_min_us)
+		{
+			CurrentRollPWM = PWM_min_us;
+		}
+
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(CurrentRollPWM));
+	}
+}
+
+void AMultiRotorDrone::MoveForwardBackward(float value)
+{
+	LandingStarted = false;
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+	if (Mode == GUIDED)
+	{
+		TargetPosition.X += MaxVelocity * dt * value;
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(TargetPosition.X));
+	}
+	else if (Mode == MANUAL)
+	{
+		if (KeepXYPosition) KeepXYPosition = false;
+		CurrentPitchPWM += RollPitchPWMRate * dt * value;
+		if (CurrentPitchPWM > PWM_max_us)
+		{
+			CurrentPitchPWM = PWM_max_us;
+		}
+
+		if (CurrentPitchPWM < PWM_min_us)
+		{
+			CurrentPitchPWM = PWM_min_us;
+		}
+	}
+}
+
+void AMultiRotorDrone::HoldXYPosition(bool X, bool Y)
+{
+	//TargetPosition.Z = State.Position.Z;
+	//TargetPosition.X = State.Position.X;
+	//TargetPosition.Y = State.Position.Y;
+	//KeepAltitude = true;
+	KeepXYPosition = true;
+	if (X) CurrentPitchPWM = 1500;
+	if (Y) CurrentRollPWM = 1500;
+	pid.Xvel_int = 0.0;
+	pid.Yvel_int = 0.0;
+}
+
+void AMultiRotorDrone::UpdateYaw(float value)
+{
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+	TargetYaw = wrap_PI(TargetYaw + YawRate * dt * value);
+	//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(TargetYaw));
+}
+
+float AMultiRotorDrone::GetJ(FVector PropLoc, float RPS, float D)
+{
+	FQuaternion orient_quat;
+	orient_quat.InitializeFromEulerAngles(State.Orientation.X, State.Orientation.Y, State.Orientation.Z);
+	Matrix33 mT = orient_quat.GetMatrix().Transposed();
+	FVector Vb = mT * (State.Velocity * -1);
+	FVector localAeroVel = Vb + FVector::CrossProduct(PQR, PropLoc);
+	FVector direction(0, 0, -1);
+	float lVeln = FVector::DotProduct(localAeroVel, direction);
+	FVector vVeln = direction * lVeln;
+	float V = vVeln.Size();
+	float J;
+	if (RPS > 0.01)
+	{
+		J = V / (RPS * D);
+	}
+	else
+	{
+		J = V / D;
+	}
+
+	return J;
+}
+
+void AMultiRotorDrone::AltitudeController(float& target_height, float& AltCmd)
+{
+	float current_height = State.Position.Z;;
+	float vert_velocity = State.Velocity.Z;
+
+	float max_rps = MaxRPM / 60;
+	float min_rps = MinRPM / 60;
+	float up_limit = 0.7 * 4 * max_rps * max_rps * Kt;
+	float low_limit = 0.1 * Mass * GRAVITY;
+
+	if(Mode == GUIDED)
+	{
+		//UE_LOG(LogTemp, Display, TEXT("Target height %f"), target_height);
+		float height_error = target_height - current_height;
+		float target_vel = height_error / MjStep;
+		float tVel = pid.Kpz * height_error;
+		float VelError = target_vel - State.Velocity.Z;
+		float tAcc = tVel - pid.Kvz * State.Velocity.Z;
+		AltCmd = -tAcc + GRAVITY * Mass;
+		//UE_LOG(LogTemp, Display, TEXT("Target Z position %f"), TargetPosition.Z);
+		//UE_LOG(LogTemp, Display, TEXT("Current Z position %f"), State.Position.Z);
+	}
+	else
+	{
+		float ratio = (CurrentThrottle - PWM_min_us) / (PWM_max_us - PWM_min_us);
+		AltCmd = ratio * MaxThrust;
+		float target_vel = -MaxVelocity + 2 * MaxVelocity * ratio;
+		//UE_LOG(LogTemp, Display, TEXT("Target Z velocity %f"), target_vel);
+		float VelError = -target_vel - State.Velocity.Z;
+		float accel_ff = VelError / MjStep;
+		pid.Zvel_int += VelError * MjStep;
+		AltCmd  = -pid.Kd_z * VelError - pid.Ki_z * pid.Zvel_int - accel_ff;
+		AltCmd *= Mass;
+		//GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, FString::SanitizeFloat(target_vel));
+	}
+
+	AltCmd = fmax(fmin(AltCmd, up_limit), low_limit);
+	
+	//UE_LOG(LogTemp, Display, TEXT("Alt CMD %f"), AltCmd);
+}
+
+void AMultiRotorDrone::YawController(float& target_yaw, float& tau_yaw)
+{
+	//UE_LOG(LogTemp, Display, TEXT("Target yaw %f"), TargetYaw);
+	float yaw = State.Orientation.Z;
+	float r = PQR.Z;
+	float er = wrap_PI(target_yaw - yaw);
+
+	tau_yaw = pid.Kp_yaw * er - pid.Kd_yaw * r;
+	float max_rps = MaxRPM / 60;
+	float min_rps = MinRPM / 60;
+	float up_limit = 2 * max_rps * max_rps * Km - 2 * min_rps * min_rps * Km;
+	float low_limit = -up_limit;
+	tau_yaw = fmax(fmin(tau_yaw, up_limit), low_limit);
+	//UE_LOG(LogTemp, Display, TEXT("Tau yaw %f"), tau_yaw);
+}
+
+void AMultiRotorDrone::RollPitchCmdController(float& Xref, float& Yref, float& roll_cmd, float& pitch_cmd)
+{
+	float posX, posY;
+	float velX, velY;
+
+	posX = State.Position.X;
+	posY = State.Position.Y;
+	velX = State.Velocity.X;
+	velY = State.Velocity.Y;
+
+	Matrix33 yaw_rot;
+	yaw_rot.FromEuler(0.0, 0.0, State.Orientation.Z);
+
+	if (Mode == GUIDED)
+	{
+		FVector PosXY(posX, posY, 0);
+		FVector XYref(Xref, Yref, 0);
+		FVector XYvel(velX, velY, 0);
+		FVector Kp(pid.Kpx, pid.Kpy, 0);
+		FVector Kd(pid.Kvx, pid.Kvy, 0);
+
+		FVector Ki(-pid.I_xy, pid.I_xy, 0.0);
+		FVector Error = yaw_rot * (XYref - PosXY);
+		//UE_LOG(LogTemp, Display, TEXT("X Y error %f %f "), Error.X, Error.Y);
+		if (fabs(Error.X) <= XYerrTol) Error.X = 0.0f;
+		if (fabs(Error.Y) <= XYerrTol) Error.Y = 0.0f;
+		pid.XY_error_int += Error * MjStep;
+		FVector TargetVelXY = Error / MjStep;
+		FVector XYvelError = TargetVelXY - XYvel;
+		FVector rp_cmd = Kp * Error + Kd * XYvel + Ki * pid.XY_error_int;
+
+		roll_cmd = fmax(fmin(rp_cmd.Y, MaxRoll), -MaxRoll);
+		pitch_cmd = fmax(fmin(rp_cmd.X, MaxPitch), -MaxPitch);
+		//UE_LOG(LogTemp, Display, TEXT("Target X position %f"), TargetPosition.X);
+		//UE_LOG(LogTemp, Display, TEXT("Current X position %f"), State.Position.X);
+		//UE_LOG(LogTemp, Display, TEXT("Target Y position %f"), TargetPosition.Y);
+		//UE_LOG(LogTemp, Display, TEXT("Current Y position %f"), State.Position.Y);
+	}
+	else
+	{
+		float roll_ratio = (CurrentRollPWM - PWM_min_us) / (PWM_max_us - PWM_min_us);
+		float pitch_ratio = (CurrentPitchPWM - PWM_min_us) / (PWM_max_us - PWM_min_us);
+
+		roll_cmd = -MaxRoll + roll_ratio * 2 * MaxRoll;
+		pitch_cmd = -MaxPitch + pitch_ratio * 2 * MaxPitch;
+
+		FQuaternion roll_pitch_quat;
+		roll_pitch_quat.InitializeFromEulerAngles(roll_cmd, pitch_cmd, 0);
+		Matrix33 roll_pitch_mat = roll_pitch_quat.GetMatrix();
+		//Matrix33 roll_pitch_mat;
+		//roll_pitch_mat.FromEuler(roll_cmd, pitch_cmd, 0);
+		FVector z_axis(0, 0, 1);
+		z_axis = roll_pitch_mat * z_axis;
+		float bx_c = z_axis.X;
+		float by_c = -z_axis.Y;
+
+		FVector euler = State.Orientation;
+		//UE_LOG(LogTemp, Display, TEXT("State Roll Pitch %f %f"), euler.X, euler.Y);
+		FQuaternion quat;
+		quat.InitializeFromEulerAngles(euler.X, euler.Y, 0);
+		Matrix33 mat = quat.GetMatrix();
+		//Matrix33 mat;
+		//mat.FromEuler(euler.X, euler.Y, 0);
+		float bx = mat(0, 2);
+		float by = mat(1, 2);
+		//UE_LOG(LogTemp, Display, TEXT("Current bx by %f %f"), bx, by);
+
+		float bx_dot = pid2.Kp_roll * (bx_c - bx);
+		float by_dot = pid2.Kp_pitch * (by_c - by);
+
+		Matrix33 mat2(mat(1, 0), -mat(0, 0), 0.0, mat(1, 1), -mat(0, 1), 0.0, 0.0, 0.0, 0.0);
+		mat2 = mat2 * (1 / mat(2, 2));
+		FVector b(bx_dot, by_dot, 0.0);
+		FVector pq_c = mat2 * b;
+		float p_c = pq_c.X;
+		float q_c = pq_c.Y;
+		//UE_LOG(LogTemp, Display, TEXT("Commanded p q : %f %f"), p_c, q_c);
+
+		float p = PQR.X;
+	    float q = PQR.Y;
+		float r = PQR.Z;
+
+		roll_cmd = pid2.Kp_p * (p_c - p);
+		pitch_cmd = pid2.Kp_q * (q_c - q);
+	}
+}
+
+void AMultiRotorDrone::AttitudeController(float& roll_cmd, float& pitch_cmd, float& roll_tau, float& pitch_tau)
+{
+	if (Mode == GUIDED)
+	{
+		FVector pr(State.Orientation.Y, State.Orientation.X, 0.0);
+		FVector pr_ref(pitch_cmd, roll_cmd, 0.0);
+		FVector qp(PQR.Y, PQR.X, 0.0);
+		//UE_LOG(LogTemp, Display, TEXT("PQR %f %f %f"), PQR.X, PQR.Y, PQR.Z);
+		FVector Kp(pid.Kpp, pid.Kpr, 0.0);
+		FVector Kd(pid.Kdp, pid.Kdr, 0.0);
+
+		FVector pr_error = pr_ref - pr;
+		//UE_LOG(LogTemp, Display, TEXT("PR error %f %f"), pr_error.X, pr_error.Y);
+		FVector prev_int = pid.pr_error_int;
+		pid.pr_error_int += pr_error * MjStep;
+		pid.pr_error_int -= prev_int * pid.AntiWU;
+
+		//FVector error_dt = (pr_error - pid.prev_pr_error) / MjStep;
+		pid.prev_pr_error = pr_error;
+
+		FVector tau_pr = Kp * pr_error + pid.I_pr * pid.pr_error_int - Kd * qp;
+		//FVector tau_pr = Kp * pr_error + pid.I_pr * pid.pr_error_int + Kd * error_dt;
+		pitch_tau = tau_pr.X;
+		roll_tau = tau_pr.Y;
+	}
+	else
+	{
+		pitch_tau = pitch_cmd;
+		roll_tau = roll_cmd;
+	}
+	//UE_LOG(LogTemp, Display, TEXT("Roll tau Pitch tau raw %f %f "), roll_tau, pitch_tau);
+
+	float max_rps = MaxRPM / 60;
+	float min_rps = MinRPM / 60;
+
+	float up_limit = 2 * max_rps * max_rps * Kt - 2 * min_rps * min_rps * Kt;
+	float low_limit = -up_limit;
+
+	pitch_tau = fmax(fmin(pitch_tau, up_limit), low_limit);
+	roll_tau = fmax(fmin(roll_tau, up_limit), low_limit);
+
+	//UE_LOG(LogTemp, Display, TEXT("Roll tau Pitch tau %f %f "), roll_tau, pitch_tau);
+}
+
+void AMultiRotorDrone::Mixer(float& tot_thrust, float& roll_tau, float& pitch_tau, float& yaw_tau)
+{
+	//UE_LOG(LogTemp, Display, TEXT("Total thrust %f"), tot_thrust);
+
+	float xf = fabs(Rotors.p1.X);
+	float yf = fabs(Rotors.p1.Y);
+	float xr = fabs(Rotors.p2.X);
+	float yr = fabs(Rotors.p2.Y);
+
+	//float w1sq = pitch_tau / (2 * Kt * (xf + xr)) - roll_tau / (2 * Kt * (yf + yr)) + (tot_thrust * xr) / (2 * (xf + xr)) + (yaw_tau * yr) / (2 * Km * (yf + yr));
+	float w1sq = pitch_tau / (2 * Kt * (xf + xr)) - roll_tau / (2 * Kt * (yf + yr)) + (tot_thrust * xr) / (2 * Kt * (xf + xr)) + (yaw_tau * yr) / (2 * Km * (yf + yr));
+	//float w2sq = roll_tau / (2 * Kt * (yf + yr)) - pitch_tau / (2 * Kt * (xf + xr)) + (tot_thrust * xf) / (2 * (xf + xr)) + (yaw_tau * yf) / (2 * Km * (yf + yr));
+	float w2sq = roll_tau / (2 * Kt * (yf + yr)) - pitch_tau / (2 * Kt * (xf + xr)) + (tot_thrust * xf) / (2 * Kt * (xf + xr)) + (yaw_tau * yf) / (2 * Km * (yf + yr));
+	//float w3sq = pitch_tau / (2 * Kt * (xf + xr)) + roll_tau / (2 * Kt * (yf + yr)) + (tot_thrust * xr) / (2 * (xf + xr)) - (yaw_tau * yr) / (2 * Km * (yf + yr));
+	float w3sq = pitch_tau / (2 * Kt * (xf + xr)) + roll_tau / (2 * Kt * (yf + yr)) + (tot_thrust * xr) / (2 * Kt * (xf + xr)) - (yaw_tau * yr) / (2 * Km * (yf + yr));
+	//float w4sq = (tot_thrust * xf) / (2 * (xf + xr)) - roll_tau / (2 * Kt * (yf + yr)) - pitch_tau / (2 * Kt * (xf + xr)) - (yaw_tau * yf) / (2 * Km * (yf + yr));
+	float w4sq = (tot_thrust * xf) / (2 * Kt * (xf + xr)) - roll_tau / (2 * Kt * (yf + yr)) - pitch_tau / (2 * Kt * (xf + xr)) - (yaw_tau * yf) / (2 * Km * (yf + yr));
+	//UE_LOG(LogTemp, Display, TEXT("Computed omegas %f %f %f %f"), w1sq, w2sq, w3sq, w4sq);
+
+	ComputedRPM[0] = w1sq;
+	ComputedRPM[1] = w2sq;
+	ComputedRPM[2] = w3sq;
+	ComputedRPM[3] = w4sq;
+
+	float min_level = (0.05 * Mass * GRAVITY) / Kt;
+	float max_level = (MaxRPM / 60) * (MaxRPM / 60);
+	float min_wsq = ComputedRPM[0];
+	for (int32 i = 1; i < 4; i++)
+	{
+		if (ComputedRPM[i] < min_wsq) min_wsq = ComputedRPM[i];
+	}
+
+	if (min_wsq < min_level)
+	{
+		float diff = min_level - min_wsq;
+		for (int32 i = 0; i < 4; i++)
+		{
+			ComputedRPM[i] += diff;
+		}
+	}
+
+	for (int32 i = 0; i < 4; i++)
+	{
+		if (ComputedRPM[i] > max_level) ComputedRPM[i] = max_level;
+	}
+
+	if (w1sq >= 0.1) RPM[0] = sqrtf(w1sq);
+	if (w2sq > 0.1) RPM[1] = sqrtf(w2sq);
+	if (w3sq > 0.1) RPM[2] = -sqrtf(w3sq);
+	if (w4sq > 0.1) RPM[3] = -sqrtf(w4sq);
+
+	ComputeForcesAndMoments();
+}
+
+void AMultiRotorDrone::ComputeForcesAndMoments()
+{
+
+	for (int32 i = 0; i < 4; i++)
+	{
+		float rps_sq = ComputedRPM[i];
+		float rps = sqrtf(rps_sq);
+		FVector prop_loc;
+		switch (i)
+		{
+		case 0:
+			prop_loc = Rotors.p1;
+			break;
+		case 1:
+			prop_loc = Rotors.p2;
+			break;
+		case 2:
+			prop_loc = Rotors.p3;
+			break;
+		case 3:
+			prop_loc = Rotors.p4;
+			break;
+		}
+
+		//float J = GetJ(prop_loc, rps, Rotors.Diameter);
+		//float Ct = Rotors.get_value(Rotors.cThrust, J);
+		//float Cq = Rotors.get_value(Rotors.cTorque, J);
+		//UE_LOG(LogTemp, Display, TEXT("Prop %d J Ct Cq %f %f %f"), i+1, J, Ct, Cq);
+		//Kt = AIR_DENCITY * Ct * powf(Rotors.Diameter, 4);
+		//Km = AIR_DENCITY * Cq * powf(Rotors.Diameter, 5);
+
+		float force = Kt * rps_sq;
+		ForcesToApply[i] = force;
+		MomentsToApply[i] = Km * rps_sq;
+		if (i == 0 || i == 1)
+		{
+			MomentsToApply[i] *= -1;
+		}
+	}
+	//UE_LOG(LogTemp, Display, TEXT("Forces %f %f %f %f"), ForcesToApply[0], ForcesToApply[1], ForcesToApply[2], ForcesToApply[3]);
+	//UE_LOG(LogTemp, Display, TEXT("Moments %f %f %f %f"), MomentsToApply[0], MomentsToApply[1], MomentsToApply[2], MomentsToApply[3]);
+}
+
+void AMultiRotorDrone::ComputedDragForce()
+{
+	FQuaternion Orient;
+	Orient.InitializeFromEulerAngles(State.Orientation.X, State.Orientation.Y, State.Orientation.Z);
+	Matrix33 Tl2b = Orient.GetMatrix().Transposed();
+	FVector Vb = Tl2b * State.Velocity;
+	double Qbar = 0.5 * AIR_DENCITY * (Vb.X * Vb.X + Vb.Y * Vb.Y + Vb.Z * Vb.Z);
+
+	double QbarX = 0.5 * AIR_DENCITY * Vb.X * Vb.X;
+	double QbarY = 0.5 * AIR_DENCITY * Vb.Y * Vb.Y;
+	double QbarZ = 0.5 * AIR_DENCITY * Vb.Z * Vb.Z;
+
+	//Vb.Normalize();
+
+	int sign = Vb.X >= 0 ? -1 : 1;
+	ComputedDrag[0] = sign * QbarX * SrefX * CdXY;
+
+	sign = Vb.Y >= 0 ? -1 : 1;
+	ComputedDrag[1] = sign * QbarY * SrefY * CdXY;
+
+	sign = Vb.Z >= 0 ? 1 : -1;
+	ComputedDrag[2] = sign * QbarZ * SrefZ * SrefZ * CdZ;
+
+	FVector drag(ComputedDrag[0], ComputedDrag[1], -ComputedDrag[2]);
+	FVector wdrag = Tl2b.Transposed() * drag;
+	//UE_LOG(LogTemp, Display, TEXT("Computed Drag %f %f %f"), wdrag[0], wdrag[1], wdrag[2]);
+}
+
+void AMultiRotorDrone::ControllerUpdate()
+{
+	float total_thrust;
+	if (LandingStarted)
+	{
+		Landing();
+	}
+	float target_height = TargetPosition.Z;
+	AltitudeController(target_height, total_thrust);
+
+	float target_yaw = TargetYaw;
+	//float tau_yaw;
+	float yaw_cmd;
+	YawController(target_yaw, yaw_cmd);
+
+	float roll_cmd = 0;
+	float pitch_cmd = 0;
+	float xref = TargetPosition.X;
+	float yref = TargetPosition.Y;
+	RollPitchCmdController(xref, yref, roll_cmd, pitch_cmd);
+	//UE_LOG(LogTemp, Display, TEXT("Roll Pitch commands %f %f"), target_roll, target_pitch);
+	float tau_roll, tau_pitch;
+	AttitudeController(roll_cmd, pitch_cmd, tau_roll, tau_pitch);
+	//BodyRateController(roll_cmd, pitch_cmd, yaw_cmd, tau_roll, tau_pitch, tau_yaw);
+
+	Mixer(total_thrust, tau_roll, tau_pitch, yaw_cmd);
+
+	//UE_LOG(LogTemp, Display, TEXT("Commanded Position %f %f %f"), TargetPosition.X, TargetPosition.Y, TargetPosition.Z);
+	//UE_LOG(LogTemp, Display, TEXT("Current Position %f %f %f"), State.Position.X, State.Position.Y, State.Position.Z);
+
+	//ComputedDragForce();
+
+	//UE_LOG(LogTemp, Display, TEXT("Motors commands %f %f %f %f"), ComputedRPM[0], ComputedRPM[1], ComputedRPM[2], ComputedRPM[3]);
+}
+
+/*
+void AMultiRotorDrone::CorrectForces()
+{
+	float minForce = ForcesToApply[0];
+
+	for (int32 i = 1; i < 4; i++)
+	{
+		if (ForcesToApply[i] < 0)
+		{
+			minForce = ForcesToApply[i];
+			//ForcesToApply[i] = 0.0;
+		}
+	}
+	//float min_per_motor = minThrust * 0.25;
+
+	float min_per_motor = 0;
+	if (minForce < min_per_motor)
+	{
+		float diff = min_per_motor - minForce;
+		for (int32 i = 0; i < 4; i++)
+		{
+			ForcesToApply[i] += diff;
+		}
+	}
+}
+*/
+
+void AMultiRotorDrone::Landing()
+{
+	static bool initialized = false;
+	static float prev_height = 0;
+	static int height_counter = 0;
+	if (!initialized)
+	{
+		GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::White, TEXT("LANDING STARTED"));
+		prev_height = State.Position.Z;
+		initialized = true;
+	}
+
+	float height = State.Position.Z;
+	if (fabsf(height - prev_height) <= 0.001)
+	{
+		height_counter += 1;
+		if (height_counter >= 1000)
+		{
+			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Green, TEXT("LANDING COMPLETED"));
+			height_counter = 0;
+			LandingStarted = false;
+			initialized = false;
+			TakeOffStarted = false;
+			OnGround = true;
+		}
+	}
+
+	float landing_speed = 3;
+	if (TargetPosition.Z < 0)
+	{
+		TargetPosition.Z += landing_speed * MjStep;
+	}
+
+	prev_height = height;
+}
+
+//CONTROLLER 2
+
+void AMultiRotorDrone::LateralController(float& x, float& dx, float& ddx, float& y, float& dy, float& ddy, float& c, float& bx_c, float& by_c)
+{
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+
+	float X = State.Position.X;
+	float Y = State.Position.Y;
+	float dX = State.Velocity.X;
+	float dY = State.Velocity.Y;
+
+	FQuaternion yaw_quat;
+	yaw_quat.InitializeFromEulerAngles(0, 0, State.Orientation.Z);
+	Matrix33 yaw_rot = yaw_quat.GetMatrix();
+
+	float x_err = x - X;
+	float dx_ff = x_err / dt;
+	float ddx_ff = dx_ff / dt;
+
+
+	float dx_err = dx_ff - dX;
+
+	float y_err = y - Y;
+	float dy_ff = y_err / dt;
+	float ddy_ff = dy_ff / dt;
+
+	float dy_err = dy_ff - dY;
+
+	//FVector pos_error(x_err, y_err, 0);
+	//FVector vel_error(dx_err, dy_err, 0);
+	//pos_error = yaw_rot * pos_error;
+	//vel_error = yaw_rot * vel_error;
+	//x_err = pos_error.X;
+	//y_err = pos_error.Y;
+	//dx_err = vel_error.X;
+	//dy_err = vel_error.Y;
+
+	float ddx_cmd = pid2.xKp * x_err + pid2.xKd * dx_ff + ddx_ff;
+	ddx_cmd = fmax(fmin(ddx_cmd, 5), -5);
+	float ddy_cmd = pid2.yKp * y_err + pid2.yKd * dy_ff + ddy_ff;
+	ddy_cmd = fmax(fmin(ddy_cmd, 5), -5);
+
+	//float z_thrust = c * Mass;
+	//float x_thrust = fabs(ddx_cmd * Mass);
+	//float y_thrust = fabs(ddy_cmd * Mass);
+
+	//FVector tot_thrust(x_thrust, y_thrust, z_thrust);
+	//double thrust = tot_thrust.Size();
+	//if (thrust > maxThrustPerMotor * 4)
+	//{
+		//tot_thrust.Normalize();
+		//tot_thrust *= (maxThrustPerMotor * 4);
+		//float thrust_margin = maxThrustPerMotor * 4 - z_thrust;
+		//x_thrust = thrust * (x_thrust / tot_thrust.Size2D());
+		//y_thrust = thrust_margin * (y_thrust / (x_thrust + y_thrust));
+		//ddx_cmd = tot_thrust.X / Mass;
+		//ddy_cmd = tot_thrust.Y / Mass;
+		//c = tot_thrust.Z / Mass;
+	//}
+
+	//int sign_x = ddx_cmd >= 0 ? 1 : -1;
+	//int sign_y = ddy_cmd >= 0 ? 1 : -1;
+
+	//ddx_cmd = (x_thrust * sign_x) / Mass;
+	//ddy_cmd = (y_thrust * sign_y) / Mass;
+
+	UE_LOG(LogTemp, Display, TEXT("X Y accel commands: %f %f"), ddx_cmd, ddy_cmd);
+
+	bx_c = -ddx_cmd / c;
+	by_c = -ddy_cmd / c;
+}
+
+void AMultiRotorDrone::RollPitchController(float& bx_c, float& by_c, float& p_c, float& q_c)
+{
+	FVector euler = State.Orientation;
+	UE_LOG(LogTemp, Display, TEXT("Carrent orientation: %f %f %f"), euler.X, euler.Y, euler.Z);
+	FQuaternion quat;
+	quat.InitializeFromEulerAngles(euler.X, euler.Y, euler.Z);
+	Matrix33 mat = quat.GetMatrix();
+	float bx = mat(0, 2);
+	float by = mat(1, 2);
+
+	float bx_dot = pid2.Kp_roll * (bx_c - bx);
+	float by_dot = pid2.Kp_pitch * (by_c - by);
+
+	Matrix33 mat2(mat(1, 0), -mat(0, 0), 0.0, mat(1, 1), -mat(0, 1), 0.0, 0.0, 0.0, 0.0);
+	mat2 = mat2 * (1 / mat(2, 2));
+	FVector b(bx_dot, by_dot, 0.0);
+	FVector pq_c = mat2 * b;
+	float max_tilt = PI / 4;
+	p_c = pq_c.X;
+	q_c = pq_c.Y;
+
+	UE_LOG(LogTemp, Display, TEXT("Commanded p q : %f %f"), p_c, q_c);
+}
+
+void AMultiRotorDrone::BodyRateController(float& p_c, float& q_c, float& r_c, float& u_bar_p, float& u_bar_q, float& u_bar_r)
+{
+	float p = PQR.X;
+	float q = PQR.Y;
+	float r = PQR.Z;
+
+	u_bar_p = pid2.Kp_p * (p_c - p);
+	u_bar_q = pid2.Kp_q * (q_c - q);
+	//u_bar_r = pid2.Kp_r * (r_c - r);
+
+	UE_LOG(LogTemp, Display, TEXT("Commanded body rates : %f %f %f"), u_bar_p, u_bar_q, u_bar_r);
+
+	float max_rps = MaxRPM / 60;
+	float min_rps = MinRPM / 60;
+	float pq_up_limit = 2 * max_rps * max_rps * Kt - 2 * min_rps * min_rps * Kt;
+	float pq_low_limit = -pq_up_limit;
+
+	float r_up_limit = 2 * max_rps * max_rps * Km - 2 * min_rps * min_rps * Km;
+	float r_low_limit = -r_up_limit;
+
+	u_bar_p = fmax(pq_low_limit, fmin(u_bar_p, pq_up_limit));
+	u_bar_q = fmax(pq_low_limit, fmin(u_bar_p, pq_up_limit));
+	//u_bar_r = fmax(r_low_limit, fmin(u_bar_p, r_up_limit));
+}
+
+void AMultiRotorDrone::HeadingController(float& yaw_c, float& yaw_tau)
+{
+	float yaw = State.Orientation.Z;
+
+	yaw_tau = pid2.Kp_yaw * (yaw_c - yaw);
+}
+
+void AMultiRotorDrone::HeightController(float& z_t, float& dz_t, float& ddz_t, float& c)
+{
+	float Z = State.Position.Z;
+	float dZ = State.Velocity.Z;
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+	float z_error = z_t - Z;
+	float dz_ff = z_error / dt;
+	float ddz_ff = dz_ff / dt;
+
+	float u_1_bar = pid2.zKp * z_error + pid2.zKd * (dz_ff - dZ) + ddz_ff;
+
+	FQuaternion quat;
+	quat.InitializeFromEulerAngles(State.Orientation.X, State.Orientation.Y, State.Orientation.Z);
+	Matrix33 mat = quat.GetMatrix();
+
+	float b_z = mat(2, 2);
+	c = (-u_1_bar - GRAVITY) / b_z;
+	if ((c + GRAVITY) * Mass > maxThrustPerMotor * 4)
+	{
+		c = ((maxThrustPerMotor * 4) / Mass) - GRAVITY;
+	}
+
+	//UE_LOG(LogTemp, Display, TEXT("Target thrust vs maxThrust %f %f"), c * Mass, maxThrustPerMotor * 4);
+}
+
+void AMultiRotorDrone::Controller2Update()
+{
+	UE_LOG(LogTemp, Display, TEXT("Target Height %f"), TargetPosition.Z);
+	float dt = UGameplayStatics::GetWorldDeltaSeconds(this);
+
+	float c = 0;
+	float target_height = TargetPosition.Z;
+	float dz_ff = MaxVelocity;
+	float ddz_ff = MaxVelocity / dt;
+	HeightController(target_height, dz_ff, ddz_ff, c);
+
+	float bx_c = 0;
+	float by_c = 0;
+	float target_x = TargetPosition.X;
+	float dx_ff = MaxVelocity;
+	float ddx_ff = MaxVelocity / dt;
+	float target_y = TargetPosition.Y;
+	float dy_ff = MaxVelocity;
+	float ddy_ff = MaxVelocity / dt;
+	LateralController(target_x, dx_ff, ddx_ff, target_y, dy_ff, ddy_ff, c, bx_c, by_c);
+
+	float p_c, q_c;
+	RollPitchController(bx_c, by_c, p_c, q_c);
+
+	float yaw_target = TargetYaw;
+	float r_c;
+	HeadingController(yaw_target, r_c);
+
+	float u_bar_p, u_bar_q, u_bar_r;
+	BodyRateController(p_c, q_c, r_c, u_bar_p, u_bar_q, u_bar_r);
+
+	//float omega_c = c / Kt;
+	//float omega_p = u_bar_p / Kt;
+	//float omega_q = u_bar_q / Kt;
+	//float omega_r = u_bar_r / Km;
+
+	ComputedRPM[0] = (0.25 * (c - u_bar_p + u_bar_q + u_bar_r)) / Kt;
+	ComputedRPM[1] = (0.25 * (c + u_bar_p - u_bar_q + u_bar_r)) / Kt;
+	ComputedRPM[2] = (0.25 * (c + u_bar_p + u_bar_q - u_bar_r)) / Kt;
+	ComputedRPM[3] = (0.25 * (c - u_bar_p - u_bar_q - u_bar_r)) / Kt;
+
+	//float w1sq = 0.25 * (omega_c - omega_p + omega_q + omega_r);
+	//float w2sq = 0.25 * (omega_c + omega_p - omega_q + omega_r);
+	//float w3sq = 0.25 * (omega_c + omega_p + omega_q - omega_r);
+	//float w4sq = 0.25 * (omega_c - omega_p - omega_q - omega_r);
+
+	//ComputedRPM[0] = w1sq;
+	//ComputedRPM[1] = w2sq;
+	//ComputedRPM[2] = w3sq;
+	//ComputedRPM[3] = w4sq;
+	/*
+	float min_rps_sq_value = 0;
+	float min_rps_sq = min_rps_sq_value;
+	for (int32 i = 0; i < 4; i++)
+	{
+		if (ComputedRPM[i] < min_rps_sq)
+		{
+			min_rps_sq = ComputedRPM[i];
+		}
+	}
+	float diff = min_rps_sq_value - min_rps_sq;
+	for (int32 i = 0; i < 4; i++)
+	{
+		ComputedRPM[i] += diff;
+	}
+	*/
+	//UE_LOG(LogTemp, Display, TEXT("Motors commands %f %f %f %f"), ComputedRPM[0], ComputedRPM[1], ComputedRPM[2], ComputedRPM[3]);
+
+	ComputeForcesAndMoments();
+}
+
+
+
+
+
+

Source/LuckyWorld/MultiRotorDrone.h

@@ -0,0 +1,366 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "GameFramework/Pawn.h"
+#include "EKF.h"
+#include "EKFilter.h"
+#include "IMUSensor.h"
+#include "GPSSensor.h"
+#include "Barometer.h"
+#include "MultiRotorDrone.generated.h"
+
+#define GYR0_LIMIT   2000
+#define ACCEL_LIMIT  16
+#define GRAVITY      9.81
+#define MF_INTECITY  31.84
+#define MF_INC       -30.89
+#define MF_DEC       -4.016
+#define AIR_DENCITY  1.1839
+
+USTRUCT(BlueprintType)
+struct FTrueDroneState
+{
+	GENERATED_BODY()
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	FVector Position;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	FVector Velocity;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	FVector Acceleration;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite)
+	FVector Orientation;
+
+	bool initialised = false;
+};
+
+UCLASS()
+class LUCKYWORLD_API AMultiRotorDrone : public APawn
+{
+	GENERATED_BODY()
+
+public:
+	// Sets default values for this pawn's properties
+	AMultiRotorDrone();
+
+	UFUNCTION(BlueprintCallable)
+	void SetImuSensor(UIMUSensor* sensor);
+
+	UFUNCTION(BlueprintCallable)
+	void SetGPSSensor(UGPSSensor* sensor);
+
+	UFUNCTION(BlueprintCallable)
+	void SetBaroSensor(UBarometer* sensor);
+
+	UFUNCTION(BlueprintCallable)
+	void SetTrueState(FTrueDroneState true_state);
+
+	UFUNCTION(BlueprintCallable)
+	void IncreaseDecreaseTargetHeight(float value);
+
+	UFUNCTION(BlueprintCallable)
+	void HoldAltitude();
+
+	UFUNCTION(BlueprintCallable)
+	void MoveLeftRight(float value);
+
+	UFUNCTION(BlueprintCallable)
+	void HoldXYPosition(bool X, bool Y);
+
+	UFUNCTION(BlueprintCallable)
+	void MoveForwardBackward(float value);
+
+	UFUNCTION(BlueprintCallable)
+	void UpdateYaw(float value);
+
+protected:
+	// Called when the game starts or when spawned
+	virtual void BeginPlay() override;
+
+	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Motors", meta = (AllowPrivateAccess = true))
+	TArray<double> RPM = { 0, 0, 0, 0 };
+
+	UPROPERTY(BlueprintReadWrite)
+	TArray<double> ForcesToApply = { 0, 0, 0, 0 };
+
+	UPROPERTY(BlueprintReadWrite)
+	TArray<double> MomentsToApply = { 0, 0, 0, 0 };
+
+	UPROPERTY(BlueprintReadWrite)
+	TArray<double> ComputedDrag = { 0, 0, 0 };
+
+	UPROPERTY(VisibleAnywhere, BlueprintReadWrite, meta = (AllowPrivateAccess = true))
+	TArray<double> GyroBuffer;
+
+	UPROPERTY(VisibleAnywhere, BlueprintReadWrite, meta = (AllowPrivateAccess = true))
+	bool LandingStarted = false;
+
+	UIMUSensor* Imu_sensor;
+	UGPSSensor* GPS_sensor;
+	UBarometer* Baro_sensor;
+
+	EKFilter StateEstimator;
+
+	struct IMUdata
+	{
+		FVector data;
+		double dt;
+	};
+
+	union
+	{
+		IMUdata RecentIMUData;
+		double IMUDataArray[4];
+	};
+
+	enum FlyingMode
+	{
+		MANUAL = 1,
+		GUIDED
+	};
+
+	bool KeepAltitude = false;
+	bool KeepXYPosition = false;
+
+	FlyingMode Mode = MANUAL;
+
+
+
+	struct PID_Controller
+	{
+		float Kpz = 30;
+		//float Kpz = 100;
+		//float Kvz = 0.3;
+		float Kvz = 30;
+		float Kd_z = 100;
+		float Ki_z = 10;
+		float Zvel_int = 0.0;
+		//float Kp_yaw = 0.004;
+		//float Kd_yaw = 0.3 * 0.004;
+		float Kp_yaw = 100;
+		float Kd_yaw = 10;
+		//float Kpx = -0.24;
+		float Kpx = -0.5;
+		//float Kpy = 0.24;
+		float Kpy = 0.5;
+		float Kvx = 1;
+		float Kvy = -1;
+		float Kd_x = -1.5;
+		float Kd_y = 1.5;
+		float Ki_x = -0.5;
+		float Ki_y = 0.5;
+		float Xvel_int = 0;
+		float Yvel_int = 0;
+		float I_xy = 0;
+		//float Kpr = 0.01;
+		float Kpr = 30;
+		//float Kpp = 0.013;
+		float Kpp = 30;
+		//float Kdr = 0.0028;
+		float Kdr = 10;
+		//float Kdp = 0.002;
+		float Kdp = 20;
+		//float I_pr = 0.01;
+		float I_pr = 0;
+		//float AntiWU = 0.001;
+		float AntiWU = 0;
+		FVector pr_error_int = { 0.0, 0.0, 0.0 };
+		FVector prev_pr_error = { 0.0, 0.0, 0.0 };
+		FVector XY_error_int = { 0.0, 0.0, 0.0 };
+		FVector prev_XY_error = { 0.0, 0.0, 0.0 };
+	}pid;
+
+	float k_throttle = 600;
+	float k_roll = 200.0;
+	float k_pitch = 200.0;
+	float k_yaw = 200.0;
+
+	float Ts2Q[16];
+
+	struct PID_Controller2
+	{
+		float zKp = 100;
+		float zKd = 80;
+		float xKp = 1.0;
+		float xKd = 0;
+		float yKp = 1.0;
+		float yKd = 0;
+		float Kp_roll = 10;
+		float Kp_pitch = 10;
+		float Kp_yaw = 10.0;
+		//float Kp_p = 100;
+		//float Kp_q = 5;
+		float Kp_p = 100;
+		float Kp_q = 100;
+		float Kp_r = 1;
+	}pid2;
+
+	struct Rotor
+	{
+		float Ct = 0.05;
+		float Cq = 0.05 / (2 * PI);
+		TArray<double> cThrust = { 0.2, 0.1, 0.3, 0.09, 0.4, 0.075, 0.5, 0.055, 0.6, 0.04 };
+		TArray<double> cTorque = { 0.2, 0.05, 0.3, 0.05, 0.4, 0.05, 0.5, 0.04, 0.6, 0.03, 0.7, 0.02, 0.8, 0.01 };
+		float Diameter = (21 * 25.4) / 1000;
+		FVector p1 = { 0.286 + 0.02, 0.363, 0.217 };
+		FVector p2 = { -0.308 + 0.02, -0.316, 0.217 };
+		FVector p3 = { 0.286 + 0.02, -0.363, 0.217 };
+		FVector p4 = { -0.308 + 0.02, 0.316, 0.217 };
+		double get_value(TArray<double> table, double key)
+		{
+			if (key <= table[0])
+				return table[1];
+			else if (key >= table[table.Num() - 2])
+				return table[table.Num() - 1];
+
+			unsigned int r = 1;
+			while (table[2 * r] < key) r++;
+
+			double x0 = table[2 * r - 2];
+			double span = table[2 * r] - x0;
+			double factor = (key - x0) / span;
+			double y0 = table[2 * r - 1];
+			return factor * (table[2 * r + 1] - y0) + y0;
+		};
+	}Rotors;
+
+	float Kt, Km;
+
+	float Mass = 6.6;
+
+	float ThrustToRPM = 100;
+
+	int NumMotors = 4;
+
+	float MinRPM = 1000;
+	float MaxRPM = 10000;
+	float PWM_min_us = 1200;
+	float PWM_max_us = 1800;
+	float ThrottleRate = 100;
+	float CurrentThrottle = 1500;
+	float CurrentRollPWM = 1500;
+	float CurrentPitchPWM = 1500;
+	float RollPitchPWMRate = 300;
+	float TargetThrust = 0;
+	float MaxThrust;
+	float XYerrTol = 0.1;
+
+	float MaxRoll = PI / 4;
+	float MaxPitch = PI / 4;
+	float MaxRPrate = PI / 6;
+
+	float ComputedRPM[4];
+
+	FVector Inertia = { 0.06, 0.06, 0.1 };
+
+	FVector TargetPosition;
+	FVector TargetVelocity = { 0.0, 0.0, 0.0 };
+	FVector TargetAccel = { 0.0, 0.0, 0.0 };
+	float MaxVelocity = 10;
+	float MaxAcceleration = 5;
+	//FVector Euler = { 0.0, 0.0, 0.0 };
+	FVector PQR = { 0.0, 0.0, 0.0 };
+
+	FVector COMLocation;
+	FTrueDroneState BodyState;
+
+	float TargetYaw;
+	float YawRate = PI / 6;
+	float TurnDir = 0.0f;
+
+	bool TakeOffStarted = false;
+	bool OnGround = true;
+
+	float minThrust;
+
+	float maxThrustPerMotor = 70 / 4;
+
+	float CdZ = 3;
+	float CdXY = 1.5;
+	float SrefZ;
+	float SrefX = 0.2 * 0.2;
+	float SrefY = 0.2 * 0.3;
+
+	FTrueDroneState TrueState;
+	FTrueDroneState State;
+	StateElements EstState;
+
+	bool use_true_state = true;
+	bool AltitudeManualControl = true;
+	bool XYmanualControl = true;
+
+	void SetCurrentState();
+
+	void EulerDt2PQR(FVector& EulerDt);
+
+	void AltitudeController(float& target_height, float& AltCmd);
+
+	void YawController(float& target_yaw, float& tau_yaw);
+
+	void RollPitchCmdController(float& Xref, float& Yref, float& roll_cmd, float& pitch_cmd);
+
+	void AttitudeController(float& roll_cmd, float& pitch_cmd, float& roll_tau, float& pitch_tau);
+
+	void Mixer(float& tot_thrust, float& roll_tau, float& pitch_tau, float& yaw_tau);
+
+	void ControllerUpdate();
+
+	void ComputeForcesAndMoments();
+
+	void ComputedDragForce();
+
+	//void CorrectForces();
+
+	void LateralController(float& x, float& dx, float& ddx, float& y, float& dy, float& ddy, float& c, float& bx_c, float& by_c);
+
+	void RollPitchController(float& bx_c, float& by_c, float& p_c, float& q_c);
+
+	void BodyRateController(float& p_c, float& q_c, float& r_c, float& u_bar_p, float& u_bar_q, float& u_bar_r);
+
+	void HeadingController(float& yaw_c, float& yaw_tau);
+
+	void HeightController(float& z_t, float& dz_t, float& ddz_t, float& c);
+
+	void Controller2Update();
+
+	float GetJ(FVector PropLoc, float RPS, float D);
+
+	void Landing();
+
+	//void StructuralToCOM();
+
+public:
+	// Called every frame
+	virtual void Tick(float DeltaTime) override;
+
+	// Called to bind functionality to input
+	virtual void SetupPlayerInputComponent(class UInputComponent* PlayerInputComponent) override;
+
+private:
+
+	//float MjStep = 1 * 0.001667;
+	float MjStep = 1 * 0.001;
+
+	float wrap_2PI(const float radian)
+	{
+		float res = fmodf(radian, 2 * PI);
+		if (res < 0) {
+			res += 2 * PI;
+		}
+		return res;
+	}
+	float wrap_PI(const float radian)
+	{
+		float res = wrap_2PI(radian);
+		if (res > PI) {
+			res -= 2 * PI;
+		}
+		return res;
+	}
+
+};

Source/LuckyWorld/Random.cpp

@@ -0,0 +1,55 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Random.h"
+#include <random>
+#include <math.h>
+
+Random::Random()
+{
+}
+
+Random::~Random()
+{
+}
+
+double Random::RandNorm(double mean, double stddev)
+{
+	
+	//if (!n2_cached)
+	//{
+		double x, y, r;
+		do
+		{
+			x = 2.0 * rand() / RAND_MAX - 1;
+			y = 2.0 * rand() / RAND_MAX - 1;
+			r = x * x + y * y;
+		} while (r == 0 || r > 1.0);
+		const double d = sqrt(-2.0 * log(r) / r);
+		const double n1 = x * d;
+		n2 = y * d;
+		const double result = n1 * stddev + mean;
+		//n2_cached = 1;
+		return result;
+	//}
+	//else
+	//{
+		//n2_cached = 0;
+		//return n2 * stddev + mean;
+	//}
+	
+
+	//std::random_device rd{};
+	//std::mt19937 gen{ rd() };
+	//std::normal_distribution dist{ mean, stddev };
+	//return dist(gen);
+}
+
+double Random::UniformRand(double min, double max)
+{
+	std::random_device rand_dev;
+	std::mt19937 generator(rand_dev());
+	std::uniform_real_distribution<double> dist(min, max);
+	return dist(generator);
+	//return 0;
+}

Source/LuckyWorld/Random.h

@@ -0,0 +1,21 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include <random>
+
+/**
+ * 
+ */
+class LUCKYWORLD_API Random
+{
+public:
+	Random();
+	~Random();
+	double RandNorm(double mean, double stddev);
+	double UniformRand(double min, double max);
+private:
+	double n2 = 0.0;
+	int n2_cached = 0;
+};