diff --git a/Content/Blueprint/DATA/datatables/DT_MainBPCharacter.uasset b/Content/Blueprint/DATA/datatables/DT_MainBPCharacter.uasset index 5b260339..2dba7fee 100644 --- a/Content/Blueprint/DATA/datatables/DT_MainBPCharacter.uasset +++ b/Content/Blueprint/DATA/datatables/DT_MainBPCharacter.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:537ebecdcff2c7975d984e514c58d8510bb241c8b088417d0ca459746c82c8c1 -size 21384 +oid sha256:b8a0f80d4de27eb946baf63ec2617c893f68fb6458d98b4b8d97f559d34fb70f +size 21699 diff --git a/Content/Blueprint/RobotPawnActors/BP_DJI300_pawn.uasset b/Content/Blueprint/RobotPawnActors/BP_DJI300_pawn.uasset new file mode 100644 index 00000000..e52cc2fb --- /dev/null +++ b/Content/Blueprint/RobotPawnActors/BP_DJI300_pawn.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac31816e0bc080ea2d7e75001d52aff77507547975f5fc4d7710fe8bf6d77fe0 +size 570782 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300.uasset b/Content/Developers/Wdev/Robots/BP_DJI300.uasset new file mode 100644 index 00000000..ba31b3ac --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e6749349044b1db02cf8a40b7651deacdb48df1864aec291dca0a308e1f40e0d +size 62230 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame.uasset new file mode 100644 index 00000000..f59529a5 --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7c0052c7bdecaf923e5156eb87ae7615ba0ff42f767fe2b7f786d9121b424583 +size 54787643 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame_Mujoco.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame_Mujoco.uasset new file mode 100644 index 00000000..db17c64b --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/frame_Mujoco.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bd2addcd03cdab087c916033ce6d15784fc40f1d8a134a1b187ad55f03d1e551 +size 813923823 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop1_Mujoco.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop1_Mujoco.uasset new file mode 100644 index 00000000..e8fe296d --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop1_Mujoco.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:82a71aee5b1c1999d09e76040f26392da0c4e5807ec6b73e12fb4f69c899b0c8 +size 9339120 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2.uasset new file mode 100644 index 00000000..9768091d --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0d215aefa8b5ce4bad30418b2c19112b0225767c462bfff03b11962af8e19a75 +size 771957 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2_Mujoco.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2_Mujoco.uasset new file mode 100644 index 00000000..c70e1e54 --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop2_Mujoco.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8a0a28c0d83d412a625b57355c3a03ba390ad0a7932b728442e3969c661e5333 +size 9350531 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3.uasset new file mode 100644 index 00000000..2634a4cb --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c6f33d6f6fc47cc2815ccd810a839e0de9a4cec9689d0c310ddd387985107ca0 +size 774216 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3_Mujoco.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3_Mujoco.uasset new file mode 100644 index 00000000..e9b2b798 --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop3_Mujoco.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:549b19d011d2ff24e2995653f906ee5c3e09208b434c5faab7e2918a2ddb1a6f +size 9254522 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4.uasset new file mode 100644 index 00000000..43a42a6a --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:991a7bf21a40d753717b02aaa5a73105d81d2129407eed620edd2c77da66e0d5 +size 763999 diff --git a/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4_Mujoco.uasset b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4_Mujoco.uasset new file mode 100644 index 00000000..9f748f6b --- /dev/null +++ b/Content/Developers/Wdev/Robots/BP_DJI300/Meshes/OBJ/prop4_Mujoco.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0e5724fc718e7904704dafc5c38f7ef8bd100fd7252ee4703c0670d51dd1d09c +size 9249494 diff --git a/Content/Map/Drone_world.umap b/Content/Map/Drone_world.umap new file mode 100644 index 00000000..e1e62c0f --- /dev/null +++ b/Content/Map/Drone_world.umap @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:134ad7447cbe69022d5086f73976ce47f576df51cd23a1cd61bcbff291b9a19b +size 789266 diff --git a/Content/luckyBot/INPUT/DroneInput/DroneControl.uasset b/Content/luckyBot/INPUT/DroneInput/DroneControl.uasset new file mode 100644 index 00000000..d42d39b9 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/DroneControl.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bee82e2bdd03a34b95bda7db5b125270e744da3ea3da30315f7f24e8383d9c69 +size 7634 diff --git a/Content/luckyBot/INPUT/DroneInput/alt_hold.uasset b/Content/luckyBot/INPUT/DroneInput/alt_hold.uasset new file mode 100644 index 00000000..8adf2c8d --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/alt_hold.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:08f285362c5fd447a487688498700a855a8a4b68f9b35983ab17e136e1347ad4 +size 1700 diff --git a/Content/luckyBot/INPUT/DroneInput/drone_backward.uasset b/Content/luckyBot/INPUT/DroneInput/drone_backward.uasset new file mode 100644 index 00000000..00f84eec --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/drone_backward.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:245dcbb7717756ec4f603e5c5cc3e411c753fe65acbd631f45efb03931019d04 +size 2057 diff --git a/Content/luckyBot/INPUT/DroneInput/drone_forward.uasset b/Content/luckyBot/INPUT/DroneInput/drone_forward.uasset new file mode 100644 index 00000000..379245c8 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/drone_forward.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f5e37d798a81feec21e2c5c58c20464de6b407125942dd0af0715bd1e4d5bf2e +size 2052 diff --git a/Content/luckyBot/INPUT/DroneInput/height_decrease.uasset b/Content/luckyBot/INPUT/DroneInput/height_decrease.uasset new file mode 100644 index 00000000..86fd6bd0 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/height_decrease.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ca9bc426dcf32b8a59a547c32ea4bc5cc7adbae68be10d95b193df02e0914ad +size 2062 diff --git a/Content/luckyBot/INPUT/DroneInput/height_increase.uasset b/Content/luckyBot/INPUT/DroneInput/height_increase.uasset new file mode 100644 index 00000000..cc0ed418 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/height_increase.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f172dfe68b0968c391a9c840baaba84a2cd594588b6f9fba50e07f1cf88777b4 +size 2062 diff --git a/Content/luckyBot/INPUT/DroneInput/move_left.uasset b/Content/luckyBot/INPUT/DroneInput/move_left.uasset new file mode 100644 index 00000000..4662f405 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/move_left.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e77c33eeb2eef62530788f42dfe361ea89c73c4439b142bee41116c249fdb405 +size 2032 diff --git a/Content/luckyBot/INPUT/DroneInput/move_right.uasset b/Content/luckyBot/INPUT/DroneInput/move_right.uasset new file mode 100644 index 00000000..dea43b04 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/move_right.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a209e70fd9945713e1a273543a3e599c3fd88847e01c496ff29e417974e696b3 +size 2037 diff --git a/Content/luckyBot/INPUT/DroneInput/turn_ccw.uasset b/Content/luckyBot/INPUT/DroneInput/turn_ccw.uasset new file mode 100644 index 00000000..3dda7faf --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/turn_ccw.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e84b91d3261b18f9486beb205c96c177c64e443496b59471410be02be5a8b2e3 +size 2027 diff --git a/Content/luckyBot/INPUT/DroneInput/turn_cw.uasset b/Content/luckyBot/INPUT/DroneInput/turn_cw.uasset new file mode 100644 index 00000000..afc4fba8 --- /dev/null +++ b/Content/luckyBot/INPUT/DroneInput/turn_cw.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34c530f96fa5b272d11eb57eeb63f69d012ac22cbdcb72e22d81c7c2637764fd +size 2022 diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/LuckyMujoco.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/LuckyMujoco.cpp index c3e26fed..55240ae1 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/LuckyMujoco.cpp +++ b/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) diff --git a/Source/LuckyWorld/Barometer.cpp b/Source/LuckyWorld/Barometer.cpp new file mode 100644 index 00000000..baed671a --- /dev/null +++ b/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); +} diff --git a/Source/LuckyWorld/Barometer.h b/Source/LuckyWorld/Barometer.h new file mode 100644 index 00000000..797ad1da --- /dev/null +++ b/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 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; +}; diff --git a/Source/LuckyWorld/EKF.cpp b/Source/LuckyWorld/EKF.cpp new file mode 100644 index 00000000..2e8bc4c1 --- /dev/null +++ b/Source/LuckyWorld/EKF.cpp @@ -0,0 +1,2276 @@ +// Fill out your copyright notice in the Description page of Project Settings. + + +#include "EKF.h" +#include "Misc/DateTime.h" + +EKF::EKF() +{ +} + +EKF::~EKF() +{ +} + +void EKF::Update(bool update) +{ + if (!readIMUData()) + { + return; + } + + UpdateVelPosQuat(); + + if (update) + { + CovariancePrediction(nullptr); + + MagFusion(); + + VelPosFusion(); + } +} + +void EKF::GetState(FStateElements& state_struct) +{ + for (int i = 0; i < 4; i++) + { + state_struct.quat[i] = statesArray[i]; + } + state_struct.velocity = FVector(statesArray[4], statesArray[5], statesArray[6]); + state_struct.position = FVector(statesArray[7], statesArray[8], statesArray[9]); + state_struct.gyro_bias = FVector(statesArray[10], statesArray[11], statesArray[12]); + state_struct.accel_bias = FVector(statesArray[13], statesArray[14], statesArray[15]); + state_struct.earth_magfield = FVector(statesArray[16], statesArray[17], statesArray[18]); + state_struct.body_magfield = FVector(statesArray[19], statesArray[20], statesArray[21]); +} + +bool EKF::InitialiseFilter() +{ + if (!readGPS() || !readBaro() || !readIMUData() || !readMagData()) + { + return false; + } + + if (!validOrigin) + { + Location origin; + origin.SetPositionGeodetic(recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt); + setOrigin(origin); + //ekfOriginHgtVar = sq(gpsHgtAccuracy); + } + + alignMagStateDeclination(); + + //YawAngle = GetMagYawAngle(); + //YawAngleErr = (PI / 180) * 5.0f; + + InitialiseVariables(); + + FVector initAccVec; + initAccVec = recentAccel.data_corrected / recentAccel.dt; + ahrsAccel = initAccVec; + float pitch = 0, roll = 0; + if (initAccVec.Size() > 0.001f) + { + initAccVec.Normalize(); + pitch = asinf(initAccVec.X); + roll = atan2f(-initAccVec.Y, -initAccVec.Z); + } + //FQuaternion quat; + StateQuat.InitializeFromEulerAngles(roll, pitch, 0.0f); + + //State.velocity = { 0, 0, 0 }; + //State.position = { 0, 0, 0 }; + //State.accel_bias = { 0, 0, 0 }; + //State.gyro_bias = { 0, 0, 0 }; + //State.body_magfield = { 0, 0, 0 }; + //State.earth_magfield = { 0, 0, 0 }; + + //State.quat = quat; + + memset(statesArray, 0.0f, num_states); + quat2arr(); + + //UE_LOG(LogTemp, Warning, TEXT("Velocity after init: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.velocity after init: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + + //UE_LOG(LogTemp, Warning, TEXT("Velocity after resets: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.velocity after resets: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + CovarianceInit(); + + //UE_LOG(LogTemp, Warning, TEXT("Velocity after covariance init: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.velocity covariance init: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + + //vertCompFiltState.pos = State.position.Z; + //vertCompFiltState.vel = State.velocity.Z; + Initialised = true; + return true; +} + +void EKF::InitialiseVariables() +{ + memset(&P[0][0], 0, sizeof(P)); + memset(&KH[0][0], 0, sizeof(KH)); + memset(&KHP[0][0], 0, sizeof(KHP)); + memset(&nextP[0][0], 0, sizeof(nextP)); + + accelBias = { 0.05, 0.05, 0.08 }; + gyroBias = { 0.349, 0.349, 0.349 }; + magOffsets = { 7.5e-06, 7.5e-06, 7.5e-06 }; + baroHgtOffset = 0.0f; + + _gyroBiasProcessNoise = 0.5; + _accelBiasProcessNoise = 5.0E-01f; + + _magEarthProcessNoise = 1.0E-03f; + _magBodyProcessNoise = 1.0E-04f; + _magNoise = 0.002f; + //_gyrNoise = 1.5E-02f; + _gyrNoise = 0.0008726646f; + //_accNoise = 3.5E-01f; + _accNoise = 0.0004f; + //_gpsVertVelNoise = 0.7f; + _gpsVertVelNoise = 0.01f; + _baroAltNoise = 2.0f; + //_gpsHorizVelNoise = 0.5f; + _gpsHorizVelNoise = 0.01f; + _gpsHorizPosNoise = 1e-5f; + _gpsVertPosNoise = 1e-5f; + //_gpsHorizPosNoise = 0.5f; + //_gpsVertPosNoise = 0.5f; + _accBiasLim = 1.0f; + _yawNoise = 0.5f; + _baroGndEffectDeadZone = 4.0f; + + gpsSpdAccuracy = 0.1f; + //gpsSpdAccuracy = 0.01f; + gpsPosAccuracy = 1.6f; + gpsHgtAccuracy = 3.0f; + gpsNoiseScaler = 1.4f; + magVarRateScale = 0.005f; + gpsNEVelVarAccScale = 0.05f; + //ekfOriginHgtVar = 0.0f; + //ekfGpsRefHgt = 0.0; + gpsPosVarAccScale = 0.05f; + + //velDotNEDfilt = { 0.0, 0.0, 0.0 }; + + hgtRate = 0.0f; + + posDownObsNoise = 2.0f * 2.0f; + + tiltErrorVariance = sq(2 * PI); +} + +void EKF::RegisterBuffer(SensorType sensor_type, TArray& buffer) +{ + switch (sensor_type) + { + case SensorType::GYRO: + GyroDataBuffer = &buffer; + GyroBufferSet = true; + break; + case SensorType::ACCEL: + AccelDataBuffer = &buffer; + AccelBufferSet = true; + break; + case SensorType::MAG: + MagDataBuffer = &buffer; + MagBufferSet = true; + break; + case SensorType::GPS: + GPSDataBuffer = &buffer; + GPSBufferSet = true; + break; + case SensorType::BARO: + BaroDataBuffer = &buffer; + BaroBufferSet = true; + break; + default: + break; + } +} +/* +template +bool EKF::readNewData(TArray& data_array, T& recent_data) +{ + if (data_array.Num() == 0) + { + return false; + } + T data; + bool newData = false; + while (data_array.Num() > 0) + { + data = data_array[0]; + if (data.timestamp > recent_data.timestamp) + { + newData = true; + break; + } + data_array.RemoveAt(0); + } + if (newData) + { + double timestamp = data.timestamp; + double dt = timestamp - recent_data.timestamp; + recent_data = data; + recent_data.timestamp = timestamp; + recent_data.dt = dt; + } + return newData; +} +*/ +bool EKF::GetNextData(TArray* sensor_buffer, double* data_array, int num) +{ + if (sensor_buffer && sensor_buffer->Num() < num) + { + return false; + } + int count = 0; + while (count < num) + { + data_array[count] = (*sensor_buffer)[0]; + sensor_buffer->RemoveAt(0); + count++; + } + return true; +} +/* +bool EKF::TestSensorReading() +{ + bool imuready = readIMUData(); + bool magready = readMagData(); + bool gpsready = readGPS(); + bool baroready = readBaro(); + + if (imuready) + { + UE_LOG(LogTemp, Display, TEXT("Recent Gyro: %f, %f, %f, %f"), recentGyro.data.X, recentGyro.data.Y, recentGyro.data.Z, recentGyro.dt); + UE_LOG(LogTemp, Display, TEXT("Recent Accel: %f, %f, %f, %f"), recentAccel.data.X, recentAccel.data.Y, recentAccel.data.Z, recentAccel.dt); + } + if (magready) + { + UE_LOG(LogTemp, Display, TEXT("Recent Mag: %f, %f, %f, %f"), recentMag.data.X, recentMag.data.Y, recentMag.data.Z, recentMag.dt); + } + if (gpsready) + { + UE_LOG(LogTemp, Display, TEXT("Recent GPS: %f, %f, %f, %f, %f, %f, %f"), recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt, recentGPS.VelocityEast, recentGPS.VelocityNorth, recentGPS.VelocityUP, recentGPS.dt); + } + if (baroready) + { + UE_LOG(LogTemp, Display, TEXT("Recent Baro: %f, %f, %f"), recentBaro.altitude, recentBaro.pressure, recentBaro.dt); + } + return true; +} +*/ +bool EKF::readIMUData() +{ + /* + if (!readNewData(AccelReadings, recentAccel) || !readNewData(GyroReadings, recentGyro)) + return false; + + recentAccel.data *= recentAccel.dt; + recentGyro.data *= recentGyro.dt; + + CorrectSensorData(recentAccel, State.accel_bias); + CorrectSensorData(recentGyro, State.gyro_bias); + return true; + */ + if (!GyroBufferSet || !AccelBufferSet) + { + return false; + } + + int32 num_dp = 0; + FVector gyro_data = { 0, 0, 0 }; + float dt = 0; + while (GetNextData(GyroDataBuffer, IMUDataArray, 4)) + { + gyro_data += RecentIMUData.data * RecentIMUData.dt; + dt += RecentIMUData.dt; + num_dp++; + } + //UE_LOG(LogTemp, Display, TEXT("Read gyro %d times"), num_dp) + if (num_dp == 0) + { + return false; + } + recentGyro.data = gyro_data / num_dp; + recentGyro.dt = dt / num_dp; + CorrectSensorData(recentGyro, gyroBias); + + num_dp = 0; + dt = 0; + FVector accel_data = { 0, 0, 0 }; + while (GetNextData(AccelDataBuffer, IMUDataArray, 4)) + { + accel_data += RecentIMUData.data * RecentIMUData.dt; + dt += RecentIMUData.dt; + num_dp++; + } + if (num_dp == 0) + { + return false; + } + recentAccel.data = accel_data / num_dp; + recentAccel.dt = dt / num_dp; + CorrectSensorData(recentAccel, accelBias); + //UE_LOG(LogTemp, Warning, TEXT("Recent accel: %f, %f, %f!"), recentAccel.data.X, recentAccel.data.Y, recentAccel.data.Z); + //UE_LOG(LogTemp, Warning, TEXT("Recent accel corrected: %f, %f, %f!"), recentAccel.data_corrected.X, recentAccel.data_corrected.Y, recentAccel.data_corrected.Z); + return true; +} + +bool EKF::readMagData() +{ + /* + if (!readNewData(MagReadings, recentMag)) + return false; + recentMag.data -= magOffsets; + recentMag.data *= 0.001; + return true; + */ + if (!MagBufferSet) + { + return false; + } + + int32 num_dp = 0; + FVector mag_data = { 0, 0, 0 }; + float dt = 0; + while (GetNextData(MagDataBuffer, IMUDataArray, 4)) + { + mag_data += RecentIMUData.data; + dt += RecentIMUData.dt; + num_dp++; + } + if (num_dp == 0) + { + return false; + } + recentMag.data = mag_data / num_dp; + recentMag.dt = dt / num_dp; + recentMag.data -= magOffsets; + //recentMag.data *= 0.001; + return true; +} + +bool EKF::readGPS() +{ + //alignMagStateDeclination(); + + if (!GPSBufferSet || !GetNextData(GPSDataBuffer, GPSDataArray, 7)) + { + return false; + } + + return true; +} + +bool EKF::readBaro() +{ + //return readNewData(BaroReadings, recentBaro); + if (!BaroBufferSet || !GetNextData(BaroDataBuffer, BaroDataArray, 3)) + { + return false; + } + return true; +} + +bool EKF::setOrigin(const Location& loc) +{ + if (validOrigin) + { + return false; + } + + EKF_origin = loc; + validOrigin = true; + + //ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.GetAlt(); + return true; +} + +void EKF::alignMagStateDeclination() +{ + FVector initMag = recentMag.data; + FVector initMagNED = prevTnb.Transposed() * initMag; + State.earth_magfield.X = initMagNED.X; + State.earth_magfield.Y = initMagNED.Y; + State.earth_magfield.Z = initMagNED.Z; + FillStateArray(); + float var_16 = P[16][16]; + float var_17 = P[17][17]; + zeroRows(P, 16, 17); + zeroCols(P, 16, 17); + P[16][16] = var_16; + P[17][17] = var_17; + + //FuseDeclination(0.1f); +} + +void EKF::CorrectSensorData(SensorReading& sensor_data, FVector bias) +{ + sensor_data.data_corrected = sensor_data.data - bias * sensor_data.dt; +} + +void EKF::GetAccelGain(FVector delVel) +{ + float filter_coef = fminf(accelFiltRatio * recentAccel.dt * tiltGain, 1.0f); + FVector accel = delVel / fmaxf(recentAccel.dt, 0.001f); + //UE_LOG(LogTemp, Warning, TEXT("Accel date: %f, %f, %f!"), recentAccel.data_corrected.X, recentAccel.data_corrected.Y, recentAccel.data_corrected.Z) + ahrsAccel = ahrsAccel * (1.0f - filter_coef) + ahrsAccel * filter_coef; + + ahrs_accel_norm = accel.Size(); + float ahrs_ng = ahrs_accel_norm / GRAVITY; + + if (ahrs_ng > 1.0f) + { + if (accel_gain <= 1.5f) + { + accel_gain = sq(3.0f - 2.0f * ahrs_ng); + } + else + { + accel_gain = 0.0f; + } + } + else if (accel_gain > 0.5) + { + accel_gain = sq(2.0f * ahrs_ng - 1.0f); + } + else + { + accel_gain = 0.0f; + } + + //UE_LOG(LogTemp, Display, TEXT("Current accel gain %f"), accel_gain); + //UE_LOG(LogTemp, Display, TEXT("Current accel norm %f"), ahrs_ng); +} + +void EKF::UpdateVelPosQuat() +{ + //UE_LOG(LogTemp, Warning, TEXT("Quaternion: %f, %f, %f, %f!"), StateQuat(0), StateQuat(1), StateQuat(2), StateQuat(3)); + //UE_LOG(LogTemp, Warning, TEXT("Gyro date: %f, %f, %f!"), recentGyro.data_corrected.X, recentGyro.data_corrected.Y, recentGyro.data_corrected.Z) + FQuaternion dq = FQuaternion::from_axis_angle(recentGyro.data_corrected); + FQuaternion nq = StateQuat * dq; + FVector n_euler = nq.GetEulerRad(); + GetAccelGain(recentAccel.data_corrected); + FVector bodyAccel = recentAccel.data_corrected / recentAccel.dt; + FVector accel_euler = { 0.0, 0.0, 0.0 }; + if (bodyAccel.Size() > 0.001f) + { + bodyAccel.Normalize(); + double pitch = asinf(bodyAccel.X); + double roll = atan2f(-bodyAccel.Y, -bodyAccel.Z); + accel_euler.X = roll; + accel_euler.Y = pitch; + accel_euler.Z = n_euler.Z; + } + FVector Euler = accel_gain * accel_euler + (1 - accel_gain) * n_euler; + StateQuat.InitializeFromEulerAngles(Euler.X, Euler.Y, Euler.Z); + StateQuat.Normalize(); + quat2arr(); + + //UE_LOG(LogTemp, Warning, TEXT("Gyro Bias: %f, %f, %f!"), State.gyro_bias.X, State.gyro_bias.Y, State.gyro_bias.Z); + //UE_LOG(LogTemp, Warning, TEXT("Accel Bias: %f, %f, %f!"), State.accel_bias.X, State.accel_bias.Y, State.accel_bias.Z); + + //UE_LOG(LogTemp, Warning, TEXT("Accel date: %f, %f, %f!"), recentAccel.data_corrected.X, recentAccel.data_corrected.Y, recentAccel.data_corrected.Z) + FVector delVelNED; + delVelNED = prevTnb.Transposed() * recentAccel.data_corrected; + + //UE_LOG(LogTemp, Warning, TEXT("delVelNED before gravity: %f, %f, %f!"), delVelNED.X, delVelNED.Y, delVelNED.Z) + delVelNED.Z += gravity * recentAccel.dt; + //UE_LOG(LogTemp, Warning, TEXT("delVelNED after gravity: %f, %f, %f!"), delVelNED.X, delVelNED.Y, delVelNED.Z) + + FVector velDotNED = delVelNED / recentAccel.dt; + float accZmag = velDotNED.Z; + //if(accZmag >= 8) + + prevTnb = StateQuat.Inverse().GetMatrix(); + + //velDotNED = delVelNED / recentAccel.dt; + //velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f; + //UE_LOG(LogTemp, Warning, TEXT("velDotNEDfilt: %f, %f, %f!"), velDotNEDfilt.X, velDotNEDfilt.Y, velDotNEDfilt.Z) + + //accNavMag = velDotNEDfilt.Size(); + //accNavMagHoriz = velDotNEDfilt.Size2D(); + + //if (accNavMagHoriz > 5.0f) + //{ + //double gain = 5.0f / accNavMagHoriz; + //delVelNED.X *= gain; + //delVelNED.Y *= gain; + //} + //UE_LOG(LogTemp, Warning, TEXT("delVelNED after gain: %f, %f, %f!"), delVelNED.X, delVelNED.Y, delVelNED.Z) + + FVector lastVelocity = State.velocity; + State.velocity += delVelNED; + //UE_LOG(LogTemp, Warning, TEXT("velocity predicted: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + State.position += (State.velocity + lastVelocity) * (recentAccel.dt * 0.5f); + //UE_LOG(LogTemp, Warning, TEXT("Position predicted: %f, %f, %f!"), State.position.X, State.position.Y, State.position.Z) + + //delAngBodyOF += recentGyro.data; + //delTimeOF += recentGyro.dt; + + FillStateArray(); + ConstrainStates(); +} + +void EKF::ConstrainStates() +{ + float dtEkfAvg = (recentAccel.dt + recentGyro.dt) * 0.5f; + for (uint8_t i = 0; i <= 3; i++) statesArray[i] = constrain_value(statesArray[i], -1.0f, 1.0f); + for (uint8_t i = 4; i <= 6; i++) statesArray[i] = constrain_value(statesArray[i], -5.0e2f, 5.0e2f); + for (uint8_t i = 7; i <= 8; i++) statesArray[i] = constrain_value(statesArray[i], -1.0e6, 1.0e6); + State.position.Z = constrain_value(State.position.Z, -4.0e4f, 1.0e4f); + for (uint8_t i = 10; i <= 12; i++) statesArray[i] = constrain_value(statesArray[i], -0.5 * dtEkfAvg, 0.5 * dtEkfAvg); + for (uint8_t i = 13; i <= 15; i++) statesArray[i] = constrain_value(statesArray[i], -_accBiasLim * dtEkfAvg, _accBiasLim * dtEkfAvg); + for (uint8_t i = 16; i <= 18; i++) statesArray[i] = constrain_value(statesArray[i], -1.0f, 1.0f); + for (uint8_t i = 19; i <= 21; i++) statesArray[i] = constrain_value(statesArray[i], -0.5f, 0.5f); + FillStateStruct(); +} + +void EKF::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 EKF::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; + } + } +} + +void EKF::FuseDeclination(float declErr) +{ + float r_decl = pow(declErr, 2); + + float magN = State.earth_magfield.X; + float magE = State.earth_magfield.Y; + + + if (magN < 1e-3f) + { + return; + } + + float magEsq = magE * magE; + float magNsq = magN * magN; + + float t1 = magEsq + magNsq; + + if (fabsf(t1) <= 1e-6f) + { + return; + } + + float H_DECL[22] = {}; + H_DECL[16] = -magE / t1; + H_DECL[17] = -magN / t1; + + float t2 = P[16][16] * magEsq + P[17][17] * magNsq + r_decl * magEsq * magEsq + r_decl * magNsq * magNsq + r_decl * magEsq * magNsq * 2.0f - P[16][17] * magE * magN - P[17][16] * magE * magN; + float t3; + if (fabsf(t2) > 1e-6f) + { + t3 = 1 / t2; + } + else + { + return; + } + + float t5 = t1 * t3; + + for (int i = 0; i < num_states; i++) + { + Kfusion[i] = -t5 * (P[i][16] * magE - P[i][17] * magN); + } + //float decl = MagDeclination(); + + float innovation = atan2f(magE, magN) - mf_declination; + + if (innovation > 0.5f) + { + innovation = 0.5f; + } + else if (innovation < -0.5f) + { + innovation = -0.5f; + } + + for (int i = 0; i < num_states; i++) + { + for (int j = 0; j <= 15; j++) + { + KH[i][j] = 0.0f; + } + KH[i][16] = Kfusion[i] * H_DECL[16]; + KH[i][17] = Kfusion[i] * H_DECL[17]; + for (int j = 18; j < num_states; j++) + { + KH[i][j] = 0.0f; + } + } + + for (int j = 0; j < num_states; j++) + { + for (int i = 0; i < num_states; i++) + { + KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j]; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < num_states; i++) + { + if (KHP[i][i] > P[i][i]) + { + healthyFusion = false; + } + } + + if (healthyFusion) + { + for (uint8_t i = 0; i < num_states; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + FillStateArray(); + for (uint8_t j = 0; j < num_states; j++) + { + statesArray[j] = statesArray[j] - Kfusion[j] * innovation; + } + FillStateStruct(); + StateQuat.Normalize(); + quat2arr(); + + ForceSymmetry(); + ConstrainVariances(); + } +} + +void EKF::ForceSymmetry() +{ + for (uint8_t i = 1; i <= stateIndexLim; i++) + { + for (uint8_t j = 0; j <= i - 1; j++) + { + float temp = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = temp; + P[j][i] = temp; + } + } +} + +float EKF::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; +} + +void EKF::FillStateStruct() +{ + arr2quat(); + State.velocity = FVector(statesArray[4], statesArray[5], statesArray[6]); + State.position = FVector(statesArray[7], statesArray[8], statesArray[9]); + State.gyro_bias = FVector(statesArray[10], statesArray[11], statesArray[12]); + State.accel_bias = FVector(statesArray[13], statesArray[14], statesArray[15]); + State.earth_magfield = FVector(statesArray[16], statesArray[17], statesArray[18]); + State.body_magfield = FVector(statesArray[19], statesArray[20], statesArray[21]); +} + +void EKF::FillStateArray() +{ + quat2arr(); + + statesArray[4] = State.velocity.X; + statesArray[5] = State.velocity.Y; + statesArray[6] = State.velocity.Z; + + statesArray[7] = State.position.X; + statesArray[8] = State.position.Y; + statesArray[9] = State.position.Z; + + statesArray[10] = State.gyro_bias.X; + statesArray[11] = State.gyro_bias.Y; + statesArray[12] = State.gyro_bias.Z; + + statesArray[13] = State.accel_bias.X; + statesArray[14] = State.accel_bias.Y; + statesArray[15] = State.accel_bias.Z; + + statesArray[16] = State.earth_magfield.X; + statesArray[17] = State.earth_magfield.Y; + statesArray[18] = State.earth_magfield.Z; + + statesArray[19] = State.body_magfield.X; + statesArray[20] = State.body_magfield.Y; + statesArray[21] = State.body_magfield.Z; +} + +float EKF::MagDeclination() +{ + float earth_decl = atan2f(State.earth_magfield.Y, State.earth_magfield.X); + return earth_decl; +} + +float EKF::GetMagYawAngle() +{ + FVector Euler = StateQuat.GetEulerRad(); + FQuaternion zero_yaw; + zero_yaw.InitializeFromEulerAngles(Euler.X, Euler.Y, 0.0f); + Matrix33 Tbn_zeroYaw = zero_yaw.GetMatrix(); + FVector magMeasNED = Tbn_zeroYaw * recentMag.data; + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.Y, magMeasNED.X) + MagDeclination()); + return yawAngMeasured; +} + +void EKF::ResetVelocity() +{ + zeroRows(P, 4, 5); + zeroCols(P, 4, 5); + + State.velocity.X = recentGPS.VelocityNorth; + State.velocity.Y = recentGPS.VelocityEast; + FillStateArray(); + P[5][5] = P[4][4] = sq(_gpsHorizVelNoise); +} + +void EKF::ResetPosition() +{ + zeroRows(P, 7, 8); + zeroCols(P, 7, 8); + + Location gpsloc; + gpsloc.SetPositionGeodetic(recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt); + FVector NEDpos = EKF_origin.GetDistanceNED(gpsloc); + State.position.X = NEDpos.X; + State.position.Y = NEDpos.Y; + FillStateArray(); + P[7][7] = P[8][8] = sq(_gpsHorizPosNoise); +} + +void EKF::ResetHeight() +{ + switch (HeightSource) + { + case SensorType::GPS: + State.position.Z = recentGPS.Alt; + break; + case SensorType::BARO: + State.position.Z = -recentBaro.altitude; + break; + default: + UE_LOG(LogTemp, Error, TEXT("Wrong height data source")); + break; + } + zeroRows(P, 9, 9); + zeroCols(P, 9, 9); + + P[9][9] = posDownObsNoise; + + State.velocity.Z = recentGPS.VelocityUP; + + zeroRows(P, 6, 6); + zeroCols(P, 6, 6); + + P[6][6] = sq(_gpsVertVelNoise); + vertVelVarClipCounter = 0; + FillStateArray(); +} + +void EKF::CovarianceInit() +{ + memset(&P[0][0], 0, sizeof(P)); + + float sq_var = 0.1f * 0.1f; + FVector rot_vec_var(sq_var, sq_var, sq_var); + + CovariancePrediction(&rot_vec_var); + + float dtEkfAvg = (recentAccel.dt + recentGyro.dt) * 0.5f; + + // velocities + P[4][4] = sq(_gpsHorizVelNoise); + P[5][5] = P[4][4]; + P[6][6] = sq(_gpsVertVelNoise); + // positions + P[7][7] = sq(_gpsHorizPosNoise); + P[8][8] = P[7][7]; + + switch (HeightSource) + { + case SensorType::GPS: + P[9][9] = sq(_gpsVertPosNoise); + break; + case SensorType::BARO: + P[9][9] = sq(_baroAltNoise); + break; + default: + UE_LOG(LogTemp, Error, TEXT("Wrong height data source")); + break; + } + // gyro delta angle biases + P[10][10] = sq(radians(2.5f * dtEkfAvg)); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + // delta velocity biases + P[13][13] = sq(0.2f * _accBiasLim * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + // earth magnetic field + P[16][16] = sq(_magNoise); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + // body magnetic field + P[19][19] = sq(_magNoise); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; +} + +void EKF::CovariancePrediction(FVector* rotVarVectPtr) +{ + float daxVar; // X axis delta angle noise variance rad^2 + float dayVar; // Y axis delta angle noise variance rad^2 + float dazVar; // Z axis delta angle noise variance rad^2 + float dvxVar; // X axis delta velocity variance noise (m/s)^2 + float dvyVar; // Y axis delta velocity variance noise (m/s)^2 + float dvzVar; // Z axis delta velocity variance noise (m/s)^2 + float dvx; // X axis delta velocity (m/s) + float dvy; // Y axis delta velocity (m/s) + float dvz; // Z axis delta velocity (m/s) + float dax; // X axis delta angle (rad) + float day; // Y axis delta angle (rad) + float daz; // Z axis delta angle (rad) + float q0; // attitude quaternion + float q1; // attitude quaternion + float q2; // attitude quaternion + float q3; // attitude quaternion + float dax_b; // X axis delta angle measurement bias (rad) + float day_b; // Y axis delta angle measurement bias (rad) + float daz_b; // Z axis delta angle measurement bias (rad) + float dvx_b; // X axis delta velocity measurement bias (rad) + float dvy_b; // Y axis delta velocity measurement bias (rad) + float dvz_b; // Z axis delta velocity measurement bias (rad) + + cov_dt = 0.5 * (recentGyro.dt + recentAccel.dt); + //double alpha = 0.1f * cov_dt; + //hgtRate = hgtRate * (1.0f - alpha) - State.velocity.Z * alpha; + + float processNoiseVariance[12] = {}; + + float gyroBPNoise = constrain_value(_gyroBiasProcessNoise, 0.0, 1.0); + //float dAngBiasVar = sq(sq(cov_dt) * gyroBPNoise); + float dAngBiasVar = gyroBPNoise; + for (uint8_t i = 0; i <= 2; i++) processNoiseVariance[i] = dAngBiasVar; + + float accelBPNoise = constrain_value(_accelBiasProcessNoise, 0.0, 1.0); + //float dVelBiasVar = sq(sq(cov_dt) * _accelBiasProcessNoise); + float dVelBiasVar = accelBPNoise; + for (uint8_t i = 3; i <= 5; i++) + { + processNoiseVariance[i] = dVelBiasVar; + } + + float magEarthPN = constrain_value(_magEarthProcessNoise, 0.0f, 1.0f); + float magEarthVar = sq(cov_dt * magEarthPN); + float magBodyPN = constrain_value(_magBodyProcessNoise, 0.0f, 1.0f); + float magBodyVar = sq(cov_dt * magBodyPN); + for (uint8_t i = 6; i <= 8; i++) processNoiseVariance[i] = magEarthVar; + for (uint8_t i = 9; i <= 11; i++) processNoiseVariance[i] = magBodyVar; + + bool quatCovResetOnly = false; + if (rotVarVectPtr != nullptr) + { + const FVector& rotVarVec = *rotVarVectPtr; + Matrix33 R_ef(rotVarVec.X, 0.0f, 0.0f, 0.0f, rotVarVec.Y, 0.0f, 0.0f, 0.0f, rotVarVec.Z); + Matrix33 Tnb = StateQuat.Inverse().GetMatrix(); + Matrix33 R_bf = Tnb * R_ef * Tnb.Transposed(); + daxVar = R_bf.A().X; + dayVar = R_bf.B().Y; + dazVar = R_bf.C().Z; + quatCovResetOnly = true; + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); + } + + float gyrNoise = constrain_value(_gyrNoise, 0.0f, 1.0f); + daxVar = dayVar = dazVar = sq(cov_dt * gyrNoise); + float accNoise = constrain_value(_accNoise, 0.0f, 5.0f); + dvxVar = dvyVar = dvzVar = sq(cov_dt * accNoise); + + dvx = recentAccel.data.X; + dvy = recentAccel.data.Y; + dvz = recentAccel.data.Z; + dax = recentGyro.data.X; + day = recentGyro.data.Y; + daz = recentGyro.data.Z; + q0 = StateQuat(0); + q1 = StateQuat(1); + q2 = StateQuat(2); + q3 = StateQuat(3); + //dax_b = State.gyro_bias.X; + dax_b = gyroBias.X; + //day_b = State.gyro_bias.Y; + day_b = gyroBias.Y; + //daz_b = State.gyro_bias.Z; + daz_b = gyroBias.Z; + //dvx_b = State.accel_bias.X; + dvx_b = accelBias.X; + //dvy_b = State.accel_bias.Y; + dvy_b = accelBias.Y; + //dvz_b = State.accel_bias.Z; + dvz_b = accelBias.Z; + + const float PS0 = sq(q1); + const float PS1 = 0.25F * daxVar; + const float PS2 = sq(q2); + const float PS3 = 0.25F * dayVar; + const float PS4 = sq(q3); + const float PS5 = 0.25F * dazVar; + const float PS6 = 0.5F * q1; + const float PS7 = 0.5F * q2; + const float PS8 = PS7 * P[10][11]; + const float PS9 = 0.5F * q3; + const float PS10 = PS9 * P[10][12]; + const float PS11 = 0.5F * dax - 0.5F * dax_b; + const float PS12 = 0.5F * day - 0.5F * day_b; + const float PS13 = 0.5F * daz - 0.5F * daz_b; + const float PS14 = PS10 - PS11 * P[1][10] - PS12 * P[2][10] - PS13 * P[3][10] + PS6 * P[10][10] + PS8 + P[0][10]; + const float PS15 = PS6 * P[10][11]; + const float PS16 = PS9 * P[11][12]; + const float PS17 = -PS11 * P[1][11] - PS12 * P[2][11] - PS13 * P[3][11] + PS15 + PS16 + PS7 * P[11][11] + P[0][11]; + const float PS18 = PS6 * P[10][12]; + const float PS19 = PS7 * P[11][12]; + const float PS20 = -PS11 * P[1][12] - PS12 * P[2][12] - PS13 * P[3][12] + PS18 + PS19 + PS9 * P[12][12] + P[0][12]; + const float PS21 = PS12 * P[1][2]; + const float PS22 = -PS13 * P[1][3]; + const float PS23 = -PS11 * P[1][1] - PS21 + PS22 + PS6 * P[1][10] + PS7 * P[1][11] + PS9 * P[1][12] + P[0][1]; + const float PS24 = -PS11 * P[1][2]; + const float PS25 = PS13 * P[2][3]; + const float PS26 = -PS12 * P[2][2] + PS24 - PS25 + PS6 * P[2][10] + PS7 * P[2][11] + PS9 * P[2][12] + P[0][2]; + const float PS27 = PS11 * P[1][3]; + const float PS28 = -PS12 * P[2][3]; + const float PS29 = -PS13 * P[3][3] - PS27 + PS28 + PS6 * P[3][10] + PS7 * P[3][11] + PS9 * P[3][12] + P[0][3]; + const float PS30 = PS11 * P[0][1]; + const float PS31 = PS12 * P[0][2]; + const float PS32 = PS13 * P[0][3]; + const float PS33 = -PS30 - PS31 - PS32 + PS6 * P[0][10] + PS7 * P[0][11] + PS9 * P[0][12] + P[0][0]; + const float PS34 = 0.5F * q0; + const float PS35 = q2 * q3; + const float PS36 = q0 * q1; + const float PS37 = q1 * q3; + const float PS38 = q0 * q2; + const float PS39 = q1 * q2; + const float PS40 = q0 * q3; + const float PS41 = 2 * PS2; + const float PS42 = 2 * PS4 - 1; + const float PS43 = PS41 + PS42; + const float PS44 = -PS11 * P[1][13] - PS12 * P[2][13] - PS13 * P[3][13] + PS6 * P[10][13] + PS7 * P[11][13] + PS9 * P[12][13] + P[0][13]; + const float PS45 = PS37 + PS38; + const float PS46 = -PS11 * P[1][15] - PS12 * P[2][15] - PS13 * P[3][15] + PS6 * P[10][15] + PS7 * P[11][15] + PS9 * P[12][15] + P[0][15]; + const float PS47 = 2 * PS46; + const float PS48 = dvy - dvy_b; + const float PS49 = PS48 * q0; + const float PS50 = dvz - dvz_b; + const float PS51 = PS50 * q1; + const float PS52 = dvx - dvx_b; + const float PS53 = PS52 * q3; + const float PS54 = PS49 - PS51 + 2 * PS53; + const float PS55 = 2 * PS29; + const float PS56 = -PS39 + PS40; + const float PS57 = -PS11 * P[1][14] - PS12 * P[2][14] - PS13 * P[3][14] + PS6 * P[10][14] + PS7 * P[11][14] + PS9 * P[12][14] + P[0][14]; + const float PS58 = 2 * PS57; + const float PS59 = PS48 * q2; + const float PS60 = PS50 * q3; + const float PS61 = PS59 + PS60; + const float PS62 = 2 * PS23; + const float PS63 = PS50 * q2; + const float PS64 = PS48 * q3; + const float PS65 = -PS64; + const float PS66 = PS63 + PS65; + const float PS67 = 2 * PS33; + const float PS68 = PS50 * q0; + const float PS69 = PS48 * q1; + const float PS70 = PS52 * q2; + const float PS71 = PS68 + PS69 - 2 * PS70; + const float PS72 = 2 * PS26; + const float PS73 = -PS11 * P[1][4] - PS12 * P[2][4] - PS13 * P[3][4] + PS6 * P[4][10] + PS7 * P[4][11] + PS9 * P[4][12] + P[0][4]; + const float PS74 = 2 * PS0; + const float PS75 = PS42 + PS74; + const float PS76 = PS39 + PS40; + const float PS77 = 2 * PS44; + const float PS78 = PS51 - PS53; + const float PS79 = -PS70; + const float PS80 = PS68 + 2 * PS69 + PS79; + const float PS81 = -PS35 + PS36; + const float PS82 = PS52 * q1; + const float PS83 = PS60 + PS82; + const float PS84 = PS52 * q0; + const float PS85 = PS63 - 2 * PS64 + PS84; + const float PS86 = -PS11 * P[1][5] - PS12 * P[2][5] - PS13 * P[3][5] + PS6 * P[5][10] + PS7 * P[5][11] + PS9 * P[5][12] + P[0][5]; + const float PS87 = PS41 + PS74 - 1; + const float PS88 = PS35 + PS36; + const float PS89 = 2 * PS63 + PS65 + PS84; + const float PS90 = -PS37 + PS38; + const float PS91 = PS59 + PS82; + const float PS92 = PS69 + PS79; + const float PS93 = PS49 - 2 * PS51 + PS53; + const float PS94 = -PS11 * P[1][6] - PS12 * P[2][6] - PS13 * P[3][6] + PS6 * P[6][10] + PS7 * P[6][11] + PS9 * P[6][12] + P[0][6]; + const float PS95 = sq(q0); + const float PS96 = -PS34 * P[10][11]; + const float PS97 = PS11 * P[0][11] - PS12 * P[3][11] + PS13 * P[2][11] - PS19 + PS9 * P[11][11] + PS96 + P[1][11]; + const float PS98 = PS13 * P[0][2]; + const float PS99 = PS12 * P[0][3]; + const float PS100 = PS11 * P[0][0] - PS34 * P[0][10] - PS7 * P[0][12] + PS9 * P[0][11] + PS98 - PS99 + P[0][1]; + const float PS101 = PS11 * P[0][2]; + const float PS102 = PS101 + PS13 * P[2][2] + PS28 - PS34 * P[2][10] - PS7 * P[2][12] + PS9 * P[2][11] + P[1][2]; + const float PS103 = PS9 * P[10][11]; + const float PS104 = PS7 * P[10][12]; + const float PS105 = PS103 - PS104 + PS11 * P[0][10] - PS12 * P[3][10] + PS13 * P[2][10] - PS34 * P[10][10] + P[1][10]; + const float PS106 = -PS34 * P[10][12]; + const float PS107 = PS106 + PS11 * P[0][12] - PS12 * P[3][12] + PS13 * P[2][12] + PS16 - PS7 * P[12][12] + P[1][12]; + const float PS108 = PS11 * P[0][3]; + const float PS109 = PS108 - PS12 * P[3][3] + PS25 - PS34 * P[3][10] - PS7 * P[3][12] + PS9 * P[3][11] + P[1][3]; + const float PS110 = PS13 * P[1][2]; + const float PS111 = PS12 * P[1][3]; + const float PS112 = PS110 - PS111 + PS30 - PS34 * P[1][10] - PS7 * P[1][12] + PS9 * P[1][11] + P[1][1]; + const float PS113 = PS11 * P[0][13] - PS12 * P[3][13] + PS13 * P[2][13] - PS34 * P[10][13] - PS7 * P[12][13] + PS9 * P[11][13] + P[1][13]; + const float PS114 = PS11 * P[0][15] - PS12 * P[3][15] + PS13 * P[2][15] - PS34 * P[10][15] - PS7 * P[12][15] + PS9 * P[11][15] + P[1][15]; + const float PS115 = 2 * PS114; + const float PS116 = 2 * PS109; + const float PS117 = PS11 * P[0][14] - PS12 * P[3][14] + PS13 * P[2][14] - PS34 * P[10][14] - PS7 * P[12][14] + PS9 * P[11][14] + P[1][14]; + const float PS118 = 2 * PS117; + const float PS119 = 2 * PS112; + const float PS120 = 2 * PS100; + const float PS121 = 2 * PS102; + const float PS122 = PS11 * P[0][4] - PS12 * P[3][4] + PS13 * P[2][4] - PS34 * P[4][10] - PS7 * P[4][12] + PS9 * P[4][11] + P[1][4]; + const float PS123 = 2 * PS113; + const float PS124 = PS11 * P[0][5] - PS12 * P[3][5] + PS13 * P[2][5] - PS34 * P[5][10] - PS7 * P[5][12] + PS9 * P[5][11] + P[1][5]; + const float PS125 = PS11 * P[0][6] - PS12 * P[3][6] + PS13 * P[2][6] - PS34 * P[6][10] - PS7 * P[6][12] + PS9 * P[6][11] + P[1][6]; + const float PS126 = -PS34 * P[11][12]; + const float PS127 = -PS10 + PS11 * P[3][12] + PS12 * P[0][12] + PS126 - PS13 * P[1][12] + PS6 * P[12][12] + P[2][12]; + const float PS128 = PS11 * P[3][3] + PS22 - PS34 * P[3][11] + PS6 * P[3][12] - PS9 * P[3][10] + PS99 + P[2][3]; + const float PS129 = PS13 * P[0][1]; + const float PS130 = PS108 + PS12 * P[0][0] - PS129 - PS34 * P[0][11] + PS6 * P[0][12] - PS9 * P[0][10] + P[0][2]; + const float PS131 = PS6 * P[11][12]; + const float PS132 = -PS103 + PS11 * P[3][11] + PS12 * P[0][11] - PS13 * P[1][11] + PS131 - PS34 * P[11][11] + P[2][11]; + const float PS133 = PS11 * P[3][10] + PS12 * P[0][10] - PS13 * P[1][10] + PS18 - PS9 * P[10][10] + PS96 + P[2][10]; + const float PS134 = PS12 * P[0][1]; + const float PS135 = -PS13 * P[1][1] + PS134 + PS27 - PS34 * P[1][11] + PS6 * P[1][12] - PS9 * P[1][10] + P[1][2]; + const float PS136 = PS11 * P[2][3]; + const float PS137 = -PS110 + PS136 + PS31 - PS34 * P[2][11] + PS6 * P[2][12] - PS9 * P[2][10] + P[2][2]; + const float PS138 = PS11 * P[3][13] + PS12 * P[0][13] - PS13 * P[1][13] - PS34 * P[11][13] + PS6 * P[12][13] - PS9 * P[10][13] + P[2][13]; + const float PS139 = PS11 * P[3][15] + PS12 * P[0][15] - PS13 * P[1][15] - PS34 * P[11][15] + PS6 * P[12][15] - PS9 * P[10][15] + P[2][15]; + const float PS140 = 2 * PS139; + const float PS141 = 2 * PS128; + const float PS142 = PS11 * P[3][14] + PS12 * P[0][14] - PS13 * P[1][14] - PS34 * P[11][14] + PS6 * P[12][14] - PS9 * P[10][14] + P[2][14]; + const float PS143 = 2 * PS142; + const float PS144 = 2 * PS135; + const float PS145 = 2 * PS130; + const float PS146 = 2 * PS137; + const float PS147 = PS11 * P[3][4] + PS12 * P[0][4] - PS13 * P[1][4] - PS34 * P[4][11] + PS6 * P[4][12] - PS9 * P[4][10] + P[2][4]; + const float PS148 = 2 * PS138; + const float PS149 = PS11 * P[3][5] + PS12 * P[0][5] - PS13 * P[1][5] - PS34 * P[5][11] + PS6 * P[5][12] - PS9 * P[5][10] + P[2][5]; + const float PS150 = PS11 * P[3][6] + PS12 * P[0][6] - PS13 * P[1][6] - PS34 * P[6][11] + PS6 * P[6][12] - PS9 * P[6][10] + P[2][6]; + const float PS151 = PS106 - PS11 * P[2][10] + PS12 * P[1][10] + PS13 * P[0][10] - PS15 + PS7 * P[10][10] + P[3][10]; + const float PS152 = PS12 * P[1][1] + PS129 + PS24 - PS34 * P[1][12] - PS6 * P[1][11] + PS7 * P[1][10] + P[1][3]; + const float PS153 = -PS101 + PS13 * P[0][0] + PS134 - PS34 * P[0][12] - PS6 * P[0][11] + PS7 * P[0][10] + P[0][3]; + const float PS154 = PS104 - PS11 * P[2][12] + PS12 * P[1][12] + PS13 * P[0][12] - PS131 - PS34 * P[12][12] + P[3][12]; + const float PS155 = -PS11 * P[2][11] + PS12 * P[1][11] + PS126 + PS13 * P[0][11] - PS6 * P[11][11] + PS8 + P[3][11]; + const float PS156 = -PS11 * P[2][2] + PS21 - PS34 * P[2][12] - PS6 * P[2][11] + PS7 * P[2][10] + PS98 + P[2][3]; + const float PS157 = PS111 - PS136 + PS32 - PS34 * P[3][12] - PS6 * P[3][11] + PS7 * P[3][10] + P[3][3]; + const float PS158 = -PS11 * P[2][13] + PS12 * P[1][13] + PS13 * P[0][13] - PS34 * P[12][13] - PS6 * P[11][13] + PS7 * P[10][13] + P[3][13]; + const float PS159 = -PS11 * P[2][15] + PS12 * P[1][15] + PS13 * P[0][15] - PS34 * P[12][15] - PS6 * P[11][15] + PS7 * P[10][15] + P[3][15]; + const float PS160 = 2 * PS159; + const float PS161 = 2 * PS157; + const float PS162 = -PS11 * P[2][14] + PS12 * P[1][14] + PS13 * P[0][14] - PS34 * P[12][14] - PS6 * P[11][14] + PS7 * P[10][14] + P[3][14]; + const float PS163 = 2 * PS162; + const float PS164 = 2 * PS152; + const float PS165 = 2 * PS153; + const float PS166 = 2 * PS156; + const float PS167 = -PS11 * P[2][4] + PS12 * P[1][4] + PS13 * P[0][4] - PS34 * P[4][12] - PS6 * P[4][11] + PS7 * P[4][10] + P[3][4]; + const float PS168 = 2 * PS158; + const float PS169 = -PS11 * P[2][5] + PS12 * P[1][5] + PS13 * P[0][5] - PS34 * P[5][12] - PS6 * P[5][11] + PS7 * P[5][10] + P[3][5]; + const float PS170 = -PS11 * P[2][6] + PS12 * P[1][6] + PS13 * P[0][6] - PS34 * P[6][12] - PS6 * P[6][11] + PS7 * P[6][10] + P[3][6]; + const float PS171 = 2 * PS45; + const float PS172 = 2 * PS56; + const float PS173 = 2 * PS61; + const float PS174 = 2 * PS66; + const float PS175 = 2 * PS71; + const float PS176 = 2 * PS54; + const float PS177 = -PS171 * P[13][15] + PS172 * P[13][14] + PS173 * P[1][13] + PS174 * P[0][13] + PS175 * P[2][13] - PS176 * P[3][13] + PS43 * P[13][13] + P[4][13]; + const float PS178 = -PS171 * P[15][15] + PS172 * P[14][15] + PS173 * P[1][15] + PS174 * P[0][15] + PS175 * P[2][15] - PS176 * P[3][15] + PS43 * P[13][15] + P[4][15]; + const float PS179 = -PS171 * P[3][15] + PS172 * P[3][14] + PS173 * P[1][3] + PS174 * P[0][3] + PS175 * P[2][3] - PS176 * P[3][3] + PS43 * P[3][13] + P[3][4]; + const float PS180 = -PS171 * P[14][15] + PS172 * P[14][14] + PS173 * P[1][14] + PS174 * P[0][14] + PS175 * P[2][14] - PS176 * P[3][14] + PS43 * P[13][14] + P[4][14]; + const float PS181 = -PS171 * P[1][15] + PS172 * P[1][14] + PS173 * P[1][1] + PS174 * P[0][1] + PS175 * P[1][2] - PS176 * P[1][3] + PS43 * P[1][13] + P[1][4]; + const float PS182 = -PS171 * P[0][15] + PS172 * P[0][14] + PS173 * P[0][1] + PS174 * P[0][0] + PS175 * P[0][2] - PS176 * P[0][3] + PS43 * P[0][13] + P[0][4]; + const float PS183 = -PS171 * P[2][15] + PS172 * P[2][14] + PS173 * P[1][2] + PS174 * P[0][2] + PS175 * P[2][2] - PS176 * P[2][3] + PS43 * P[2][13] + P[2][4]; + const float PS184 = 4 * dvyVar; + const float PS185 = 4 * dvzVar; + const float PS186 = -PS171 * P[4][15] + PS172 * P[4][14] + PS173 * P[1][4] + PS174 * P[0][4] + PS175 * P[2][4] - PS176 * P[3][4] + PS43 * P[4][13] + P[4][4]; + const float PS187 = 2 * PS177; + const float PS188 = 2 * PS182; + const float PS189 = 2 * PS181; + const float PS190 = 2 * PS81; + const float PS191 = 2 * PS183; + const float PS192 = 2 * PS179; + const float PS193 = 2 * PS76; + const float PS194 = PS43 * dvxVar; + const float PS195 = PS75 * dvyVar; + const float PS196 = -PS171 * P[5][15] + PS172 * P[5][14] + PS173 * P[1][5] + PS174 * P[0][5] + PS175 * P[2][5] - PS176 * P[3][5] + PS43 * P[5][13] + P[4][5]; + const float PS197 = 2 * PS88; + const float PS198 = PS87 * dvzVar; + const float PS199 = 2 * PS90; + const float PS200 = -PS171 * P[6][15] + PS172 * P[6][14] + PS173 * P[1][6] + PS174 * P[0][6] + PS175 * P[2][6] - PS176 * P[3][6] + PS43 * P[6][13] + P[4][6]; + const float PS201 = 2 * PS83; + const float PS202 = 2 * PS78; + const float PS203 = 2 * PS85; + const float PS204 = 2 * PS80; + const float PS205 = PS190 * P[14][15] - PS193 * P[13][14] + PS201 * P[2][14] - PS202 * P[0][14] + PS203 * P[3][14] - PS204 * P[1][14] + PS75 * P[14][14] + P[5][14]; + const float PS206 = PS190 * P[13][15] - PS193 * P[13][13] + PS201 * P[2][13] - PS202 * P[0][13] + PS203 * P[3][13] - PS204 * P[1][13] + PS75 * P[13][14] + P[5][13]; + const float PS207 = PS190 * P[0][15] - PS193 * P[0][13] + PS201 * P[0][2] - PS202 * P[0][0] + PS203 * P[0][3] - PS204 * P[0][1] + PS75 * P[0][14] + P[0][5]; + const float PS208 = PS190 * P[1][15] - PS193 * P[1][13] + PS201 * P[1][2] - PS202 * P[0][1] + PS203 * P[1][3] - PS204 * P[1][1] + PS75 * P[1][14] + P[1][5]; + const float PS209 = PS190 * P[15][15] - PS193 * P[13][15] + PS201 * P[2][15] - PS202 * P[0][15] + PS203 * P[3][15] - PS204 * P[1][15] + PS75 * P[14][15] + P[5][15]; + const float PS210 = PS190 * P[2][15] - PS193 * P[2][13] + PS201 * P[2][2] - PS202 * P[0][2] + PS203 * P[2][3] - PS204 * P[1][2] + PS75 * P[2][14] + P[2][5]; + const float PS211 = PS190 * P[3][15] - PS193 * P[3][13] + PS201 * P[2][3] - PS202 * P[0][3] + PS203 * P[3][3] - PS204 * P[1][3] + PS75 * P[3][14] + P[3][5]; + const float PS212 = 4 * dvxVar; + const float PS213 = PS190 * P[5][15] - PS193 * P[5][13] + PS201 * P[2][5] - PS202 * P[0][5] + PS203 * P[3][5] - PS204 * P[1][5] + PS75 * P[5][14] + P[5][5]; + const float PS214 = 2 * PS89; + const float PS215 = 2 * PS91; + const float PS216 = 2 * PS92; + const float PS217 = 2 * PS93; + const float PS218 = PS190 * P[6][15] - PS193 * P[6][13] + PS201 * P[2][6] - PS202 * P[0][6] + PS203 * P[3][6] - PS204 * P[1][6] + PS75 * P[6][14] + P[5][6]; + const float PS219 = -PS197 * P[14][15] + PS199 * P[13][15] - PS214 * P[2][15] + PS215 * P[3][15] + PS216 * P[0][15] + PS217 * P[1][15] + PS87 * P[15][15] + P[6][15]; + const float PS220 = -PS197 * P[14][14] + PS199 * P[13][14] - PS214 * P[2][14] + PS215 * P[3][14] + PS216 * P[0][14] + PS217 * P[1][14] + PS87 * P[14][15] + P[6][14]; + const float PS221 = -PS197 * P[13][14] + PS199 * P[13][13] - PS214 * P[2][13] + PS215 * P[3][13] + PS216 * P[0][13] + PS217 * P[1][13] + PS87 * P[13][15] + P[6][13]; + const float PS222 = -PS197 * P[6][14] + PS199 * P[6][13] - PS214 * P[2][6] + PS215 * P[3][6] + PS216 * P[0][6] + PS217 * P[1][6] + PS87 * P[6][15] + P[6][6]; + + nextP[0][0] = PS0 * PS1 - PS11 * PS23 - PS12 * PS26 - PS13 * PS29 + PS14 * PS6 + PS17 * PS7 + PS2 * PS3 + PS20 * PS9 + PS33 + PS4 * PS5; + nextP[0][1] = -PS1 * PS36 + PS11 * PS33 - PS12 * PS29 + PS13 * PS26 - PS14 * PS34 + PS17 * PS9 - PS20 * PS7 + PS23 + PS3 * PS35 - PS35 * PS5; + nextP[1][1] = PS1 * PS95 + PS100 * PS11 + PS102 * PS13 - PS105 * PS34 - PS107 * PS7 - PS109 * PS12 + PS112 + PS2 * PS5 + PS3 * PS4 + PS9 * PS97; + nextP[0][2] = -PS1 * PS37 + PS11 * PS29 + PS12 * PS33 - PS13 * PS23 - PS14 * PS9 - PS17 * PS34 + PS20 * PS6 + PS26 - PS3 * PS38 + PS37 * PS5; + nextP[1][2] = PS1 * PS40 + PS100 * PS12 + PS102 - PS105 * PS9 + PS107 * PS6 + PS109 * PS11 - PS112 * PS13 - PS3 * PS40 - PS34 * PS97 - PS39 * PS5; + nextP[2][2] = PS0 * PS5 + PS1 * PS4 + PS11 * PS128 + PS12 * PS130 + PS127 * PS6 - PS13 * PS135 - PS132 * PS34 - PS133 * PS9 + PS137 + PS3 * PS95; + nextP[0][3] = PS1 * PS39 - PS11 * PS26 + PS12 * PS23 + PS13 * PS33 + PS14 * PS7 - PS17 * PS6 - PS20 * PS34 + PS29 - PS3 * PS39 - PS40 * PS5; + nextP[1][3] = -PS1 * PS38 + PS100 * PS13 - PS102 * PS11 + PS105 * PS7 - PS107 * PS34 + PS109 + PS112 * PS12 - PS3 * PS37 + PS38 * PS5 - PS6 * PS97; + nextP[2][3] = -PS1 * PS35 - PS11 * PS137 + PS12 * PS135 - PS127 * PS34 + PS128 + PS13 * PS130 - PS132 * PS6 + PS133 * PS7 + PS3 * PS36 - PS36 * PS5; + nextP[3][3] = PS0 * PS3 + PS1 * PS2 - PS11 * PS156 + PS12 * PS152 + PS13 * PS153 + PS151 * PS7 - PS154 * PS34 - PS155 * PS6 + PS157 + PS5 * PS95; + + + + if (quatCovResetOnly) + { + // covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP + // to lower and upper half in P + for (uint8_t row = 0; row <= 3; row++) + { + // copy diagonals + P[row][row] = constrain_value(nextP[row][row], 0.0f, 1.0f); + // copy off diagonals + for (uint8_t column = 0; column < row; column++) + { + P[row][column] = P[column][row] = nextP[column][row]; + } + } + calcTiltErrorVariance(); + return; + } + + nextP[0][4] = PS43 * PS44 - PS45 * PS47 - PS54 * PS55 + PS56 * PS58 + PS61 * PS62 + PS66 * PS67 + PS71 * PS72 + PS73; + nextP[1][4] = PS113 * PS43 - PS115 * PS45 - PS116 * PS54 + PS118 * PS56 + PS119 * PS61 + PS120 * PS66 + PS121 * PS71 + PS122; + nextP[2][4] = PS138 * PS43 - PS140 * PS45 - PS141 * PS54 + PS143 * PS56 + PS144 * PS61 + PS145 * PS66 + PS146 * PS71 + PS147; + nextP[3][4] = PS158 * PS43 - PS160 * PS45 - PS161 * PS54 + PS163 * PS56 + PS164 * PS61 + PS165 * PS66 + PS166 * PS71 + PS167; + nextP[4][4] = -PS171 * PS178 + PS172 * PS180 + PS173 * PS181 + PS174 * PS182 + PS175 * PS183 - PS176 * PS179 + PS177 * PS43 + PS184 * sq(PS56) + PS185 * sq(PS45) + PS186 + sq(PS43) * dvxVar; + nextP[0][5] = PS47 * PS81 + PS55 * PS85 + PS57 * PS75 - PS62 * PS80 - PS67 * PS78 + PS72 * PS83 - PS76 * PS77 + PS86; + nextP[1][5] = PS115 * PS81 + PS116 * PS85 + PS117 * PS75 - PS119 * PS80 - PS120 * PS78 + PS121 * PS83 - PS123 * PS76 + PS124; + nextP[2][5] = PS140 * PS81 + PS141 * PS85 + PS142 * PS75 - PS144 * PS80 - PS145 * PS78 + PS146 * PS83 - PS148 * PS76 + PS149; + nextP[3][5] = PS160 * PS81 + PS161 * PS85 + PS162 * PS75 - PS164 * PS80 - PS165 * PS78 + PS166 * PS83 - PS168 * PS76 + PS169; + nextP[4][5] = PS172 * PS195 + PS178 * PS190 + PS180 * PS75 - PS185 * PS45 * PS81 - PS187 * PS76 - PS188 * PS78 - PS189 * PS80 + PS191 * PS83 + PS192 * PS85 - PS193 * PS194 + PS196; + nextP[5][5] = PS185 * sq(PS81) + PS190 * PS209 - PS193 * PS206 + PS201 * PS210 - PS202 * PS207 + PS203 * PS211 - PS204 * PS208 + PS205 * PS75 + PS212 * sq(PS76) + PS213 + sq(PS75) * dvyVar; + nextP[0][6] = PS46 * PS87 + PS55 * PS91 - PS58 * PS88 + PS62 * PS93 + PS67 * PS92 - PS72 * PS89 + PS77 * PS90 + PS94; + nextP[1][6] = PS114 * PS87 + PS116 * PS91 - PS118 * PS88 + PS119 * PS93 + PS120 * PS92 - PS121 * PS89 + PS123 * PS90 + PS125; + nextP[2][6] = PS139 * PS87 + PS141 * PS91 - PS143 * PS88 + PS144 * PS93 + PS145 * PS92 - PS146 * PS89 + PS148 * PS90 + PS150; + nextP[3][6] = PS159 * PS87 + PS161 * PS91 - PS163 * PS88 + PS164 * PS93 + PS165 * PS92 - PS166 * PS89 + PS168 * PS90 + PS170; + nextP[4][6] = -PS171 * PS198 + PS178 * PS87 - PS180 * PS197 - PS184 * PS56 * PS88 + PS187 * PS90 + PS188 * PS92 + PS189 * PS93 - PS191 * PS89 + PS192 * PS91 + PS194 * PS199 + PS200; + nextP[5][6] = PS190 * PS198 - PS195 * PS197 - PS197 * PS205 + PS199 * PS206 + PS207 * PS216 + PS208 * PS217 + PS209 * PS87 - PS210 * PS214 + PS211 * PS215 - PS212 * PS76 * PS90 + PS218; + nextP[6][6] = PS184 * sq(PS88) - PS197 * PS220 + PS199 * PS221 + PS212 * sq(PS90) - PS214 * (-PS197 * P[2][14] + PS199 * P[2][13] - PS214 * P[2][2] + PS215 * P[2][3] + PS216 * P[0][2] + PS217 * P[1][2] + PS87 * P[2][15] + P[2][6]) + PS215 * (-PS197 * P[3][14] + PS199 * P[3][13] - PS214 * P[2][3] + PS215 * P[3][3] + PS216 * P[0][3] + PS217 * P[1][3] + PS87 * P[3][15] + P[3][6]) + PS216 * (-PS197 * P[0][14] + PS199 * P[0][13] - PS214 * P[0][2] + PS215 * P[0][3] + PS216 * P[0][0] + PS217 * P[0][1] + PS87 * P[0][15] + P[0][6]) + PS217 * (-PS197 * P[1][14] + PS199 * P[1][13] - PS214 * P[1][2] + PS215 * P[1][3] + PS216 * P[0][1] + PS217 * P[1][1] + PS87 * P[1][15] + P[1][6]) + PS219 * PS87 + PS222 + sq(PS87) * dvzVar; + nextP[0][7] = -PS11 * P[1][7] - PS12 * P[2][7] - PS13 * P[3][7] + PS6 * P[7][10] + PS7 * P[7][11] + PS73 * cov_dt + PS9 * P[7][12] + P[0][7]; + nextP[1][7] = PS11 * P[0][7] - PS12 * P[3][7] + PS122 * cov_dt + PS13 * P[2][7] - PS34 * P[7][10] - PS7 * P[7][12] + PS9 * P[7][11] + P[1][7]; + nextP[2][7] = PS11 * P[3][7] + PS12 * P[0][7] - PS13 * P[1][7] + PS147 * cov_dt - PS34 * P[7][11] + PS6 * P[7][12] - PS9 * P[7][10] + P[2][7]; + nextP[3][7] = -PS11 * P[2][7] + PS12 * P[1][7] + PS13 * P[0][7] + PS167 * cov_dt - PS34 * P[7][12] - PS6 * P[7][11] + PS7 * P[7][10] + P[3][7]; + nextP[4][7] = -PS171 * P[7][15] + PS172 * P[7][14] + PS173 * P[1][7] + PS174 * P[0][7] + PS175 * P[2][7] - PS176 * P[3][7] + PS186 * cov_dt + PS43 * P[7][13] + P[4][7]; + nextP[5][7] = PS190 * P[7][15] - PS193 * P[7][13] + PS201 * P[2][7] - PS202 * P[0][7] + PS203 * P[3][7] - PS204 * P[1][7] + PS75 * P[7][14] + P[5][7] + cov_dt * (PS190 * P[4][15] - PS193 * P[4][13] + PS201 * P[2][4] - PS202 * P[0][4] + PS203 * P[3][4] - PS204 * P[1][4] + PS75 * P[4][14] + P[4][5]); + nextP[6][7] = -PS197 * P[7][14] + PS199 * P[7][13] - PS214 * P[2][7] + PS215 * P[3][7] + PS216 * P[0][7] + PS217 * P[1][7] + PS87 * P[7][15] + P[6][7] + cov_dt * (-PS197 * P[4][14] + PS199 * P[4][13] - PS214 * P[2][4] + PS215 * P[3][4] + PS216 * P[0][4] + PS217 * P[1][4] + PS87 * P[4][15] + P[4][6]); + nextP[7][7] = P[4][7] * cov_dt + P[7][7] + cov_dt * (P[4][4] * cov_dt + P[4][7]); + nextP[0][8] = -PS11 * P[1][8] - PS12 * P[2][8] - PS13 * P[3][8] + PS6 * P[8][10] + PS7 * P[8][11] + PS86 * cov_dt + PS9 * P[8][12] + P[0][8]; + nextP[1][8] = PS11 * P[0][8] - PS12 * P[3][8] + PS124 * cov_dt + PS13 * P[2][8] - PS34 * P[8][10] - PS7 * P[8][12] + PS9 * P[8][11] + P[1][8]; + nextP[2][8] = PS11 * P[3][8] + PS12 * P[0][8] - PS13 * P[1][8] + PS149 * cov_dt - PS34 * P[8][11] + PS6 * P[8][12] - PS9 * P[8][10] + P[2][8]; + nextP[3][8] = -PS11 * P[2][8] + PS12 * P[1][8] + PS13 * P[0][8] + PS169 * cov_dt - PS34 * P[8][12] - PS6 * P[8][11] + PS7 * P[8][10] + P[3][8]; + nextP[4][8] = -PS171 * P[8][15] + PS172 * P[8][14] + PS173 * P[1][8] + PS174 * P[0][8] + PS175 * P[2][8] - PS176 * P[3][8] + PS196 * cov_dt + PS43 * P[8][13] + P[4][8]; + nextP[5][8] = PS190 * P[8][15] - PS193 * P[8][13] + PS201 * P[2][8] - PS202 * P[0][8] + PS203 * P[3][8] - PS204 * P[1][8] + PS213 * cov_dt + PS75 * P[8][14] + P[5][8]; + nextP[6][8] = -PS197 * P[8][14] + PS199 * P[8][13] - PS214 * P[2][8] + PS215 * P[3][8] + PS216 * P[0][8] + PS217 * P[1][8] + PS87 * P[8][15] + P[6][8] + cov_dt * (-PS197 * P[5][14] + PS199 * P[5][13] - PS214 * P[2][5] + PS215 * P[3][5] + PS216 * P[0][5] + PS217 * P[1][5] + PS87 * P[5][15] + P[5][6]); + nextP[7][8] = P[4][8] * cov_dt + P[7][8] + cov_dt * (P[4][5] * cov_dt + P[5][7]); + nextP[8][8] = P[5][8] * cov_dt + P[8][8] + cov_dt * (P[5][5] * cov_dt + P[5][8]); + nextP[0][9] = -PS11 * P[1][9] - PS12 * P[2][9] - PS13 * P[3][9] + PS6 * P[9][10] + PS7 * P[9][11] + PS9 * P[9][12] + PS94 * cov_dt + P[0][9]; + nextP[1][9] = PS11 * P[0][9] - PS12 * P[3][9] + PS125 * cov_dt + PS13 * P[2][9] - PS34 * P[9][10] - PS7 * P[9][12] + PS9 * P[9][11] + P[1][9]; + nextP[2][9] = PS11 * P[3][9] + PS12 * P[0][9] - PS13 * P[1][9] + PS150 * cov_dt - PS34 * P[9][11] + PS6 * P[9][12] - PS9 * P[9][10] + P[2][9]; + nextP[3][9] = -PS11 * P[2][9] + PS12 * P[1][9] + PS13 * P[0][9] + PS170 * cov_dt - PS34 * P[9][12] - PS6 * P[9][11] + PS7 * P[9][10] + P[3][9]; + nextP[4][9] = -PS171 * P[9][15] + PS172 * P[9][14] + PS173 * P[1][9] + PS174 * P[0][9] + PS175 * P[2][9] - PS176 * P[3][9] + PS200 * cov_dt + PS43 * P[9][13] + P[4][9]; + nextP[5][9] = PS190 * P[9][15] - PS193 * P[9][13] + PS201 * P[2][9] - PS202 * P[0][9] + PS203 * P[3][9] - PS204 * P[1][9] + PS218 * cov_dt + PS75 * P[9][14] + P[5][9]; + nextP[6][9] = -PS197 * P[9][14] + PS199 * P[9][13] - PS214 * P[2][9] + PS215 * P[3][9] + PS216 * P[0][9] + PS217 * P[1][9] + PS222 * cov_dt + PS87 * P[9][15] + P[6][9]; + nextP[7][9] = P[4][9] * cov_dt + P[7][9] + cov_dt * (P[4][6] * cov_dt + P[6][7]); + nextP[8][9] = P[5][9] * cov_dt + P[8][9] + cov_dt * (P[5][6] * cov_dt + P[6][8]); + nextP[9][9] = P[6][9] * cov_dt + P[9][9] + cov_dt * (P[6][6] * cov_dt + P[6][9]); + nextP[0][10] = PS14; + nextP[1][10] = PS105; + nextP[2][10] = PS133; + nextP[3][10] = PS151; + nextP[4][10] = -PS171 * P[10][15] + PS172 * P[10][14] + PS173 * P[1][10] + PS174 * P[0][10] + PS175 * P[2][10] - PS176 * P[3][10] + PS43 * P[10][13] + P[4][10]; + nextP[5][10] = PS190 * P[10][15] - PS193 * P[10][13] + PS201 * P[2][10] - PS202 * P[0][10] + PS203 * P[3][10] - PS204 * P[1][10] + PS75 * P[10][14] + P[5][10]; + nextP[6][10] = -PS197 * P[10][14] + PS199 * P[10][13] - PS214 * P[2][10] + PS215 * P[3][10] + PS216 * P[0][10] + PS217 * P[1][10] + PS87 * P[10][15] + P[6][10]; + nextP[7][10] = P[4][10] * cov_dt + P[7][10]; + nextP[8][10] = P[5][10] * cov_dt + P[8][10]; + nextP[9][10] = P[6][10] * cov_dt + P[9][10]; + nextP[10][10] = P[10][10]; + nextP[0][11] = PS17; + nextP[1][11] = PS97; + nextP[2][11] = PS132; + nextP[3][11] = PS155; + nextP[4][11] = -PS171 * P[11][15] + PS172 * P[11][14] + PS173 * P[1][11] + PS174 * P[0][11] + PS175 * P[2][11] - PS176 * P[3][11] + PS43 * P[11][13] + P[4][11]; + nextP[5][11] = PS190 * P[11][15] - PS193 * P[11][13] + PS201 * P[2][11] - PS202 * P[0][11] + PS203 * P[3][11] - PS204 * P[1][11] + PS75 * P[11][14] + P[5][11]; + nextP[6][11] = -PS197 * P[11][14] + PS199 * P[11][13] - PS214 * P[2][11] + PS215 * P[3][11] + PS216 * P[0][11] + PS217 * P[1][11] + PS87 * P[11][15] + P[6][11]; + nextP[7][11] = P[4][11] * cov_dt + P[7][11]; + nextP[8][11] = P[5][11] * cov_dt + P[8][11]; + nextP[9][11] = P[6][11] * cov_dt + P[9][11]; + nextP[10][11] = P[10][11]; + nextP[11][11] = P[11][11]; + nextP[0][12] = PS20; + nextP[1][12] = PS107; + nextP[2][12] = PS127; + nextP[3][12] = PS154; + nextP[4][12] = -PS171 * P[12][15] + PS172 * P[12][14] + PS173 * P[1][12] + PS174 * P[0][12] + PS175 * P[2][12] - PS176 * P[3][12] + PS43 * P[12][13] + P[4][12]; + nextP[5][12] = PS190 * P[12][15] - PS193 * P[12][13] + PS201 * P[2][12] - PS202 * P[0][12] + PS203 * P[3][12] - PS204 * P[1][12] + PS75 * P[12][14] + P[5][12]; + nextP[6][12] = -PS197 * P[12][14] + PS199 * P[12][13] - PS214 * P[2][12] + PS215 * P[3][12] + PS216 * P[0][12] + PS217 * P[1][12] + PS87 * P[12][15] + P[6][12]; + nextP[7][12] = P[4][12] * cov_dt + P[7][12]; + nextP[8][12] = P[5][12] * cov_dt + P[8][12]; + nextP[9][12] = P[6][12] * cov_dt + P[9][12]; + nextP[10][12] = P[10][12]; + nextP[11][12] = P[11][12]; + nextP[12][12] = P[12][12]; + nextP[0][13] = PS44; + nextP[1][13] = PS113; + nextP[2][13] = PS138; + nextP[3][13] = PS158; + nextP[4][13] = PS177; + nextP[5][13] = PS206; + nextP[6][13] = PS221; + nextP[7][13] = P[4][13] * cov_dt + P[7][13]; + nextP[8][13] = P[5][13] * cov_dt + P[8][13]; + nextP[9][13] = P[6][13] * cov_dt + P[9][13]; + nextP[10][13] = P[10][13]; + nextP[11][13] = P[11][13]; + nextP[12][13] = P[12][13]; + nextP[13][13] = P[13][13]; + nextP[0][14] = PS57; + nextP[1][14] = PS117; + nextP[2][14] = PS142; + nextP[3][14] = PS162; + nextP[4][14] = PS180; + nextP[5][14] = PS205; + nextP[6][14] = PS220; + nextP[7][14] = P[4][14] * cov_dt + P[7][14]; + nextP[8][14] = P[5][14] * cov_dt + P[8][14]; + nextP[9][14] = P[6][14] * cov_dt + P[9][14]; + nextP[10][14] = P[10][14]; + nextP[11][14] = P[11][14]; + nextP[12][14] = P[12][14]; + nextP[13][14] = P[13][14]; + nextP[14][14] = P[14][14]; + nextP[0][15] = PS46; + nextP[1][15] = PS114; + nextP[2][15] = PS139; + nextP[3][15] = PS159; + nextP[4][15] = PS178; + nextP[5][15] = PS209; + nextP[6][15] = PS219; + nextP[7][15] = P[4][15] * cov_dt + P[7][15]; + nextP[8][15] = P[5][15] * cov_dt + P[8][15]; + nextP[9][15] = P[6][15] * cov_dt + P[9][15]; + nextP[10][15] = P[10][15]; + nextP[11][15] = P[11][15]; + nextP[12][15] = P[12][15]; + nextP[13][15] = P[13][15]; + nextP[14][15] = P[14][15]; + nextP[15][15] = P[15][15]; + nextP[0][16] = -PS11 * P[1][16] - PS12 * P[2][16] - PS13 * P[3][16] + PS6 * P[10][16] + PS7 * P[11][16] + PS9 * P[12][16] + P[0][16]; + nextP[1][16] = PS11 * P[0][16] - PS12 * P[3][16] + PS13 * P[2][16] - PS34 * P[10][16] - PS7 * P[12][16] + PS9 * P[11][16] + P[1][16]; + nextP[2][16] = PS11 * P[3][16] + PS12 * P[0][16] - PS13 * P[1][16] - PS34 * P[11][16] + PS6 * P[12][16] - PS9 * P[10][16] + P[2][16]; + nextP[3][16] = -PS11 * P[2][16] + PS12 * P[1][16] + PS13 * P[0][16] - PS34 * P[12][16] - PS6 * P[11][16] + PS7 * P[10][16] + P[3][16]; + nextP[4][16] = -PS171 * P[15][16] + PS172 * P[14][16] + PS173 * P[1][16] + PS174 * P[0][16] + PS175 * P[2][16] - PS176 * P[3][16] + PS43 * P[13][16] + P[4][16]; + nextP[5][16] = PS190 * P[15][16] - PS193 * P[13][16] + PS201 * P[2][16] - PS202 * P[0][16] + PS203 * P[3][16] - PS204 * P[1][16] + PS75 * P[14][16] + P[5][16]; + nextP[6][16] = -PS197 * P[14][16] + PS199 * P[13][16] - PS214 * P[2][16] + PS215 * P[3][16] + PS216 * P[0][16] + PS217 * P[1][16] + PS87 * P[15][16] + P[6][16]; + nextP[7][16] = P[4][16] * cov_dt + P[7][16]; + nextP[8][16] = P[5][16] * cov_dt + P[8][16]; + nextP[9][16] = P[6][16] * cov_dt + P[9][16]; + nextP[10][16] = P[10][16]; + nextP[11][16] = P[11][16]; + nextP[12][16] = P[12][16]; + nextP[13][16] = P[13][16]; + nextP[14][16] = P[14][16]; + nextP[15][16] = P[15][16]; + nextP[16][16] = P[16][16]; + nextP[0][17] = -PS11 * P[1][17] - PS12 * P[2][17] - PS13 * P[3][17] + PS6 * P[10][17] + PS7 * P[11][17] + PS9 * P[12][17] + P[0][17]; + nextP[1][17] = PS11 * P[0][17] - PS12 * P[3][17] + PS13 * P[2][17] - PS34 * P[10][17] - PS7 * P[12][17] + PS9 * P[11][17] + P[1][17]; + nextP[2][17] = PS11 * P[3][17] + PS12 * P[0][17] - PS13 * P[1][17] - PS34 * P[11][17] + PS6 * P[12][17] - PS9 * P[10][17] + P[2][17]; + nextP[3][17] = -PS11 * P[2][17] + PS12 * P[1][17] + PS13 * P[0][17] - PS34 * P[12][17] - PS6 * P[11][17] + PS7 * P[10][17] + P[3][17]; + nextP[4][17] = -PS171 * P[15][17] + PS172 * P[14][17] + PS173 * P[1][17] + PS174 * P[0][17] + PS175 * P[2][17] - PS176 * P[3][17] + PS43 * P[13][17] + P[4][17]; + nextP[5][17] = PS190 * P[15][17] - PS193 * P[13][17] + PS201 * P[2][17] - PS202 * P[0][17] + PS203 * P[3][17] - PS204 * P[1][17] + PS75 * P[14][17] + P[5][17]; + nextP[6][17] = -PS197 * P[14][17] + PS199 * P[13][17] - PS214 * P[2][17] + PS215 * P[3][17] + PS216 * P[0][17] + PS217 * P[1][17] + PS87 * P[15][17] + P[6][17]; + nextP[7][17] = P[4][17] * cov_dt + P[7][17]; + nextP[8][17] = P[5][17] * cov_dt + P[8][17]; + nextP[9][17] = P[6][17] * cov_dt + P[9][17]; + nextP[10][17] = P[10][17]; + nextP[11][17] = P[11][17]; + nextP[12][17] = P[12][17]; + nextP[13][17] = P[13][17]; + nextP[14][17] = P[14][17]; + nextP[15][17] = P[15][17]; + nextP[16][17] = P[16][17]; + nextP[17][17] = P[17][17]; + nextP[0][18] = -PS11 * P[1][18] - PS12 * P[2][18] - PS13 * P[3][18] + PS6 * P[10][18] + PS7 * P[11][18] + PS9 * P[12][18] + P[0][18]; + nextP[1][18] = PS11 * P[0][18] - PS12 * P[3][18] + PS13 * P[2][18] - PS34 * P[10][18] - PS7 * P[12][18] + PS9 * P[11][18] + P[1][18]; + nextP[2][18] = PS11 * P[3][18] + PS12 * P[0][18] - PS13 * P[1][18] - PS34 * P[11][18] + PS6 * P[12][18] - PS9 * P[10][18] + P[2][18]; + nextP[3][18] = -PS11 * P[2][18] + PS12 * P[1][18] + PS13 * P[0][18] - PS34 * P[12][18] - PS6 * P[11][18] + PS7 * P[10][18] + P[3][18]; + nextP[4][18] = -PS171 * P[15][18] + PS172 * P[14][18] + PS173 * P[1][18] + PS174 * P[0][18] + PS175 * P[2][18] - PS176 * P[3][18] + PS43 * P[13][18] + P[4][18]; + nextP[5][18] = PS190 * P[15][18] - PS193 * P[13][18] + PS201 * P[2][18] - PS202 * P[0][18] + PS203 * P[3][18] - PS204 * P[1][18] + PS75 * P[14][18] + P[5][18]; + nextP[6][18] = -PS197 * P[14][18] + PS199 * P[13][18] - PS214 * P[2][18] + PS215 * P[3][18] + PS216 * P[0][18] + PS217 * P[1][18] + PS87 * P[15][18] + P[6][18]; + nextP[7][18] = P[4][18] * cov_dt + P[7][18]; + nextP[8][18] = P[5][18] * cov_dt + P[8][18]; + nextP[9][18] = P[6][18] * cov_dt + P[9][18]; + nextP[10][18] = P[10][18]; + nextP[11][18] = P[11][18]; + nextP[12][18] = P[12][18]; + nextP[13][18] = P[13][18]; + nextP[14][18] = P[14][18]; + nextP[15][18] = P[15][18]; + nextP[16][18] = P[16][18]; + nextP[17][18] = P[17][18]; + nextP[18][18] = P[18][18]; + nextP[0][19] = -PS11 * P[1][19] - PS12 * P[2][19] - PS13 * P[3][19] + PS6 * P[10][19] + PS7 * P[11][19] + PS9 * P[12][19] + P[0][19]; + nextP[1][19] = PS11 * P[0][19] - PS12 * P[3][19] + PS13 * P[2][19] - PS34 * P[10][19] - PS7 * P[12][19] + PS9 * P[11][19] + P[1][19]; + nextP[2][19] = PS11 * P[3][19] + PS12 * P[0][19] - PS13 * P[1][19] - PS34 * P[11][19] + PS6 * P[12][19] - PS9 * P[10][19] + P[2][19]; + nextP[3][19] = -PS11 * P[2][19] + PS12 * P[1][19] + PS13 * P[0][19] - PS34 * P[12][19] - PS6 * P[11][19] + PS7 * P[10][19] + P[3][19]; + nextP[4][19] = -PS171 * P[15][19] + PS172 * P[14][19] + PS173 * P[1][19] + PS174 * P[0][19] + PS175 * P[2][19] - PS176 * P[3][19] + PS43 * P[13][19] + P[4][19]; + nextP[5][19] = PS190 * P[15][19] - PS193 * P[13][19] + PS201 * P[2][19] - PS202 * P[0][19] + PS203 * P[3][19] - PS204 * P[1][19] + PS75 * P[14][19] + P[5][19]; + nextP[6][19] = -PS197 * P[14][19] + PS199 * P[13][19] - PS214 * P[2][19] + PS215 * P[3][19] + PS216 * P[0][19] + PS217 * P[1][19] + PS87 * P[15][19] + P[6][19]; + nextP[7][19] = P[4][19] * cov_dt + P[7][19]; + nextP[8][19] = P[5][19] * cov_dt + P[8][19]; + nextP[9][19] = P[6][19] * cov_dt + P[9][19]; + nextP[10][19] = P[10][19]; + nextP[11][19] = P[11][19]; + nextP[12][19] = P[12][19]; + nextP[13][19] = P[13][19]; + nextP[14][19] = P[14][19]; + nextP[15][19] = P[15][19]; + nextP[16][19] = P[16][19]; + nextP[17][19] = P[17][19]; + nextP[18][19] = P[18][19]; + nextP[19][19] = P[19][19]; + nextP[0][20] = -PS11 * P[1][20] - PS12 * P[2][20] - PS13 * P[3][20] + PS6 * P[10][20] + PS7 * P[11][20] + PS9 * P[12][20] + P[0][20]; + nextP[1][20] = PS11 * P[0][20] - PS12 * P[3][20] + PS13 * P[2][20] - PS34 * P[10][20] - PS7 * P[12][20] + PS9 * P[11][20] + P[1][20]; + nextP[2][20] = PS11 * P[3][20] + PS12 * P[0][20] - PS13 * P[1][20] - PS34 * P[11][20] + PS6 * P[12][20] - PS9 * P[10][20] + P[2][20]; + nextP[3][20] = -PS11 * P[2][20] + PS12 * P[1][20] + PS13 * P[0][20] - PS34 * P[12][20] - PS6 * P[11][20] + PS7 * P[10][20] + P[3][20]; + nextP[4][20] = -PS171 * P[15][20] + PS172 * P[14][20] + PS173 * P[1][20] + PS174 * P[0][20] + PS175 * P[2][20] - PS176 * P[3][20] + PS43 * P[13][20] + P[4][20]; + nextP[5][20] = PS190 * P[15][20] - PS193 * P[13][20] + PS201 * P[2][20] - PS202 * P[0][20] + PS203 * P[3][20] - PS204 * P[1][20] + PS75 * P[14][20] + P[5][20]; + nextP[6][20] = -PS197 * P[14][20] + PS199 * P[13][20] - PS214 * P[2][20] + PS215 * P[3][20] + PS216 * P[0][20] + PS217 * P[1][20] + PS87 * P[15][20] + P[6][20]; + nextP[7][20] = P[4][20] * cov_dt + P[7][20]; + nextP[8][20] = P[5][20] * cov_dt + P[8][20]; + nextP[9][20] = P[6][20] * cov_dt + P[9][20]; + nextP[10][20] = P[10][20]; + nextP[11][20] = P[11][20]; + nextP[12][20] = P[12][20]; + nextP[13][20] = P[13][20]; + nextP[14][20] = P[14][20]; + nextP[15][20] = P[15][20]; + nextP[16][20] = P[16][20]; + nextP[17][20] = P[17][20]; + nextP[18][20] = P[18][20]; + nextP[19][20] = P[19][20]; + nextP[20][20] = P[20][20]; + nextP[0][21] = -PS11 * P[1][21] - PS12 * P[2][21] - PS13 * P[3][21] + PS6 * P[10][21] + PS7 * P[11][21] + PS9 * P[12][21] + P[0][21]; + nextP[1][21] = PS11 * P[0][21] - PS12 * P[3][21] + PS13 * P[2][21] - PS34 * P[10][21] - PS7 * P[12][21] + PS9 * P[11][21] + P[1][21]; + nextP[2][21] = PS11 * P[3][21] + PS12 * P[0][21] - PS13 * P[1][21] - PS34 * P[11][21] + PS6 * P[12][21] - PS9 * P[10][21] + P[2][21]; + nextP[3][21] = -PS11 * P[2][21] + PS12 * P[1][21] + PS13 * P[0][21] - PS34 * P[12][21] - PS6 * P[11][21] + PS7 * P[10][21] + P[3][21]; + nextP[4][21] = -PS171 * P[15][21] + PS172 * P[14][21] + PS173 * P[1][21] + PS174 * P[0][21] + PS175 * P[2][21] - PS176 * P[3][21] + PS43 * P[13][21] + P[4][21]; + nextP[5][21] = PS190 * P[15][21] - PS193 * P[13][21] + PS201 * P[2][21] - PS202 * P[0][21] + PS203 * P[3][21] - PS204 * P[1][21] + PS75 * P[14][21] + P[5][21]; + nextP[6][21] = -PS197 * P[14][21] + PS199 * P[13][21] - PS214 * P[2][21] + PS215 * P[3][21] + PS216 * P[0][21] + PS217 * P[1][21] + PS87 * P[15][21] + P[6][21]; + nextP[7][21] = P[4][21] * cov_dt + P[7][21]; + nextP[8][21] = P[5][21] * cov_dt + P[8][21]; + nextP[9][21] = P[6][21] * cov_dt + P[9][21]; + nextP[10][21] = P[10][21]; + nextP[11][21] = P[11][21]; + nextP[12][21] = P[12][21]; + nextP[13][21] = P[13][21]; + nextP[14][21] = P[14][21]; + nextP[15][21] = P[15][21]; + nextP[16][21] = P[16][21]; + nextP[17][21] = P[17][21]; + nextP[18][21] = P[18][21]; + nextP[19][21] = P[19][21]; + nextP[20][21] = P[20][21]; + nextP[21][21] = P[21][21]; + + for (uint8_t i = 10; i < num_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoiseVariance[i - 10]; + } + + //UE_LOG(LogTemp, Display, TEXT("nextP prediction %f %f %f %f"), nextP[0][0], nextP[0][1], nextP[0][2], nextP[0][3]) + //UE_LOG(LogTemp, Display, TEXT("nextP prediction %f %f %f %f"), nextP[1][0], nextP[1][1], nextP[1][2], nextP[1][3]) + //UE_LOG(LogTemp, Display, TEXT("nextP prediction %f %f %f %f"), nextP[2][0], nextP[2][1], nextP[2][2], nextP[2][3]) + //UE_LOG(LogTemp, Display, TEXT("nextP prediction %f %f %f %f"), nextP[3][0], nextP[3][1], nextP[3][2], nextP[3][3]) + + if ((P[7][7] + P[8][8]) > 1e4f) + { + for (uint8_t i = 7; i <= 8; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + for (uint8_t row = 0; row < num_states; row++) + { + // copy diagonals + P[row][row] = nextP[row][row]; + // copy off diagonals + for (uint8_t column = 0; column < row; column++) + { + P[row][column] = P[column][row] = nextP[column][row]; + } + } + + ConstrainVariances(); + + if (vertVelVarClipCounter > 0) + { + vertVelVarClipCounter--; + } + + calcTiltErrorVariance(); + + //UE_LOG(LogTemp, Display, TEXT("P prediction %f %f %f %f"), P[0][0], P[0][1], P[0][2], P[0][3]) + //UE_LOG(LogTemp, Display, TEXT("P prediction %f %f %f %f"), P[1][0], P[1][1], P[1][2], P[1][3]) + //UE_LOG(LogTemp, Display, TEXT("P prediction %f %f %f %f"), P[2][0], P[2][1], P[2][2], P[2][3]) +} + +void EKF::ConstrainVariances() +{ + for (uint8_t i = 0; i <= 3; i++) P[i][i] = constrain_value(P[i][i], 0.0, 1.0); // attitude error + for (uint8_t i = 4; i <= 5; i++) P[i][i] = constrain_value(P[i][i], 1e-4, 1.0e3); // NE velocity + + if (P[6][6] < 1e-4) + { + P[6][6] = 1e-4; + } + + float dtEkfAvg = 0.5 * (recentGyro.dt + recentAccel.dt); + + int ekf_rate_hz = uint32_t(1.0 / dtEkfAvg); + vertVelVarClipCounter += ekf_rate_hz; + if (vertVelVarClipCounter > 5 * ekf_rate_hz) + { + zeroRows(P, 6, 6); + zeroCols(P, 6, 6); + P[6][6] = sq(_gpsVertVelNoise); + vertVelVarClipCounter = 0; + } + + for (uint8_t i = 7; i <= 9; i++) P[i][i] = constrain_value(P[i][i], 1E-4, 1.0e6); + + for (uint8_t i = 10; i <= 12; i++) P[i][i] = constrain_value(P[i][i], 0.0f, sq(0.175 * dtEkfAvg)); + + const float minSafeStateVar = 5E-9; + float maxStateVar = 0.0F; + bool resetRequired = false; + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) + { + if (P[stateIndex][stateIndex] > maxStateVar) + { + maxStateVar = P[stateIndex][stateIndex]; + } + else if (P[stateIndex][stateIndex] < minSafeStateVar) + { + resetRequired = true; + } + } + // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must + // not exceed 100 and the minimum variance must not fall below the target minimum + float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minSafeStateVar); + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) + { + P[stateIndex][stateIndex] = constrain_value(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg)); + } + + // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero + if (resetRequired) + { + // reset all delta velocity bias covariances + zeroCols(P, 13, 15); + zeroRows(P, 13, 15); + // set all delta velocity bias variances to initial values and zero bias states + P[13][13] = sq(0.2f * _accBiasLim * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + State.accel_bias = { 0.0, 0.0, 0.0 }; + } + + for (uint8_t i = 16; i <= 18; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.01f); // earth magnetic field + for (uint8_t i = 19; i <= 21; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.01f); // body magnetic field +} + +void EKF::calcTiltErrorVariance() +{ + const float& q0 = StateQuat(0); + const float& q1 = StateQuat(1); + const float& q2 = StateQuat(2); + const float& q3 = StateQuat(3); + + const float PS1 = q0 * q1 + q2 * q3; + const float PS2 = q1 * PS1; + const float PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); + const float PS5 = q0 * PS4; + const float PS6 = 2 * PS2 + PS5; + const float PS8 = PS1 * q2; + const float PS10 = PS4 * q3; + const float PS11 = PS10 + 2 * PS8; + const float PS12 = PS1 * q3; + const float PS13 = PS4 * q2; + const float PS14 = -2 * PS12 + PS13; + const float PS15 = PS1 * q0; + const float PS16 = q1 * PS4; + const float PS17 = 2 * PS15 - PS16; + const float PS18 = q0 * q2 - q1 * q3; + const float PS19 = PS18 * q2; + const float PS20 = 2 * PS19 + PS5; + const float PS22 = q1 * PS18; + const float PS23 = -PS10 + 2 * PS22; + const float PS25 = PS18 * q3; + const float PS26 = PS16 + 2 * PS25; + const float PS28 = PS18 * q0; + const float PS29 = -PS13 + 2 * PS28; + const float PS32 = PS12 + PS28; + const float PS33 = PS19 + PS2; + const float PS34 = PS15 - PS25; + const float PS35 = PS22 - PS8; + + tiltErrorVariance = 4 * sq(PS11) * P[2][2] + 4 * sq(PS14) * P[3][3] + 4 * sq(PS17) * P[0][0] + 4 * sq(PS6) * P[1][1]; + tiltErrorVariance += 4 * sq(PS20) * P[2][2] + 4 * sq(PS23) * P[1][1] + 4 * sq(PS26) * P[3][3] + 4 * sq(PS29) * P[0][0]; + tiltErrorVariance += 16 * sq(PS32) * P[1][1] + 16 * sq(PS33) * P[3][3] + 16 * sq(PS34) * P[2][2] + 16 * sq(PS35) * P[0][0]; + + tiltErrorVariance = constrain_value(tiltErrorVariance, 0.0f, sq(radians(30.0f))); +} + +void EKF::MagFusion() +{ + if (!readMagData()) + { + return; + } + + setYawFromMag(); + //resetMagFieldStates(); + + fuseEulerYaw(); + + FuseDeclination(0.34f); + + FuseMagnetometer(); +} + +void EKF::setYawFromMag() +{ + FVector Euler = StateQuat.GetEulerRad(); + FQuaternion zero_yaw; + zero_yaw.InitializeFromEulerAngles(Euler.X, Euler.Y, 0.0f); + Matrix33 Tbn_zeroYaw = zero_yaw.GetMatrix(); + FVector magMeasNED = Tbn_zeroYaw * recentMag.data; + //float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.Y, magMeasNED.X) + MagDeclination()); + //float decl = atan2f(State.earth_magfield.Y, State.earth_magfield.X); + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.Y, magMeasNED.X) + mf_declination); + //UE_LOG(LogTemp, Display, TEXT("Yaw angle measured: %f"), yawAngMeasured) + resetQuatStateYawOnly(yawAngMeasured, sq(fmax(_yawNoise, 1.0e-2f))); +} + +void EKF::resetQuatStateYawOnly(float yaw, float yawVariance) +{ + FQuaternion quatBeforeReset = StateQuat; + FVector Euler = StateQuat.GetEulerRad(); + StateQuat.InitializeFromEulerAngles(Euler.X, Euler.Y, yaw); + quat2arr(); + prevTnb = StateQuat.Inverse().GetMatrix(); + float deltaYaw = wrap_PI(yaw - Euler.Z); + FVector angleErrVarVec = FVector(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, yawVariance); + CovariancePrediction(&angleErrVarVec); +} + +void EKF::resetMagFieldStates() +{ + //State.earth_magfield = prevTnb.Transposed() * recentMag.data; + alignMagStateDeclination(); + zeroRows(P, 18, 21); + zeroCols(P, 18, 21); + P[18][18] = sq(_magNoise); + P[19][19] = P[18][18]; + P[20][20] = P[18][18]; + P[21][21] = P[18][18]; +} + +void EKF::FuseMagnetometer() +{ + const float q0 = StateQuat(0); + const float q1 = StateQuat(1); + const float q2 = StateQuat(2); + const float q3 = StateQuat(3); + const float magN = State.earth_magfield.X; + const float magE = State.earth_magfield.Y; + const float magD = State.earth_magfield.Z; + const float magXbias = State.body_magfield.X; + const float magYbias = State.body_magfield.Y; + const float magZbias = State.body_magfield.Z; + + Matrix33 DCM = StateQuat.GetMatrix(); + FVector MagPred = DCM * State.earth_magfield; + + innovMag = MagPred - recentMag.data; + + //UE_LOG(LogTemp, Display, TEXT("Magnetometer innovation %f %f %f"), innovMag.X, innovMag.Y, innovMag.Z); + + //const float R_MAG = sq(constrain_value(_magNoise, 0.01f, 0.5f)) + sq(magVarRateScale * recentGyro.data.Size() / recentGyro.dt); + const float R_MAG = _magNoise; + const Vector9 SH_MAG{ + 2.0f * magD * q3 + 2.0f * magE * q2 + 2.0f * magN * q1, + 2.0f * magD * q0 - 2.0f * magE * q1 + 2.0f * magN * q2, + 2.0f * magD * q1 + 2.0f * magE * q0 - 2.0f * magN * q3, + sq(q3), + sq(q2), + sq(q1), + sq(q0), + 2.0f * magN * q0, + 2.0f * magE * q3 + }; + + varInnovMag[0] = (P[19][19] + R_MAG + P[1][19] * SH_MAG[0] - P[2][19] * SH_MAG[1] + P[3][19] * SH_MAG[2] - P[16][19] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f * q0 * q3 + 2.0f * q1 * q2) * (P[19][17] + P[1][17] * SH_MAG[0] - P[2][17] * SH_MAG[1] + P[3][17] * SH_MAG[2] - P[16][17] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][17] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (2.0f * q0 * q2 - 2.0f * q1 * q3) * (P[19][18] + P[1][18] * SH_MAG[0] - P[2][18] * SH_MAG[1] + P[3][18] * SH_MAG[2] - P[16][18] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][18] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[19][0] + P[1][0] * SH_MAG[0] - P[2][0] * SH_MAG[1] + P[3][0] * SH_MAG[2] - P[16][0] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][0] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[17][19] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][19] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + SH_MAG[0] * (P[19][1] + P[1][1] * SH_MAG[0] - P[2][1] * SH_MAG[1] + P[3][1] * SH_MAG[2] - P[16][1] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][1] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - SH_MAG[1] * (P[19][2] + P[1][2] * SH_MAG[0] - P[2][2] * SH_MAG[1] + P[3][2] * SH_MAG[2] - P[16][2] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][2] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[2] * (P[19][3] + P[1][3] * SH_MAG[0] - P[2][3] * SH_MAG[1] + P[3][3] * SH_MAG[2] - P[16][3] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][3] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) * (P[19][16] + P[1][16] * SH_MAG[0] - P[2][16] * SH_MAG[1] + P[3][16] * SH_MAG[2] - P[16][16] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][16] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[0][19] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + //if (varInnovMag[0] < R_MAG) + //{ + //CovarianceInit(); + //return; + //} + varInnovMag[1] = (P[20][20] + R_MAG + P[0][20] * SH_MAG[2] + P[1][20] * SH_MAG[1] + P[2][20] * SH_MAG[0] - P[17][20] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f * q0 * q3 - 2.0f * q1 * q2) * (P[20][16] + P[0][16] * SH_MAG[2] + P[1][16] * SH_MAG[1] + P[2][16] * SH_MAG[0] - P[17][16] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][16] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (2.0f * q0 * q1 + 2.0f * q2 * q3) * (P[20][18] + P[0][18] * SH_MAG[2] + P[1][18] * SH_MAG[1] + P[2][18] * SH_MAG[0] - P[17][18] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][18] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[20][3] + P[0][3] * SH_MAG[2] + P[1][3] * SH_MAG[1] + P[2][3] * SH_MAG[0] - P[17][3] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][3] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - P[16][20] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][20] * (2.0f * q0 * q1 + 2.0f * q2 * q3) + SH_MAG[2] * (P[20][0] + P[0][0] * SH_MAG[2] + P[1][0] * SH_MAG[1] + P[2][0] * SH_MAG[0] - P[17][0] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][0] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[1] * (P[20][1] + P[0][1] * SH_MAG[2] + P[1][1] * SH_MAG[1] + P[2][1] * SH_MAG[0] - P[17][1] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][1] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[0] * (P[20][2] + P[0][2] * SH_MAG[2] + P[1][2] * SH_MAG[1] + P[2][2] * SH_MAG[0] - P[17][2] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][2] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) * (P[20][17] + P[0][17] * SH_MAG[2] + P[1][17] * SH_MAG[1] + P[2][17] * SH_MAG[0] - P[17][17] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][17] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - P[3][20] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + //if (varInnovMag[1] < R_MAG) + //{ + //CovarianceInit(); + //return; + //} + varInnovMag[2] = (P[21][21] + R_MAG + P[0][21] * SH_MAG[1] - P[1][21] * SH_MAG[2] + P[3][21] * SH_MAG[0] + P[18][21] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f * q0 * q2 + 2.0f * q1 * q3) * (P[21][16] + P[0][16] * SH_MAG[1] - P[1][16] * SH_MAG[2] + P[3][16] * SH_MAG[0] + P[18][16] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][16] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (2.0f * q0 * q1 - 2.0f * q2 * q3) * (P[21][17] + P[0][17] * SH_MAG[1] - P[1][17] * SH_MAG[2] + P[3][17] * SH_MAG[0] + P[18][17] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][17] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[21][2] + P[0][2] * SH_MAG[1] - P[1][2] * SH_MAG[2] + P[3][2] * SH_MAG[0] + P[18][2] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][2] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[16][21] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][21] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + SH_MAG[1] * (P[21][0] + P[0][0] * SH_MAG[1] - P[1][0] * SH_MAG[2] + P[3][0] * SH_MAG[0] + P[18][0] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][0] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - SH_MAG[2] * (P[21][1] + P[0][1] * SH_MAG[1] - P[1][1] * SH_MAG[2] + P[3][1] * SH_MAG[0] + P[18][1] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][1] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[0] * (P[21][3] + P[0][3] * SH_MAG[1] - P[1][3] * SH_MAG[2] + P[3][3] * SH_MAG[0] + P[18][3] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][3] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) * (P[21][18] + P[0][18] * SH_MAG[1] - P[1][18] * SH_MAG[2] + P[3][18] * SH_MAG[0] + P[18][18] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][18] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[2][21] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + //if (varInnovMag[2] < R_MAG) + //{ + //CovarianceInit(); + //return; + //} + + Vector22 H_MAG; + + for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) + { + if (obsIndex == 0) + { + for (uint8_t i = 0; i < num_states; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = -SH_MAG[1]; + H_MAG[3] = SH_MAG[2]; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2.0f * q0 * q3 + 2.0f * q1 * q2; + H_MAG[18] = 2.0f * q1 * q3 - 2.0f * q0 * q2; + H_MAG[19] = 1.0f; + H_MAG[20] = 0.0f; + H_MAG[21] = 0.0f; + + const Vector5 SK_MX{ + 1.0f / varInnovMag[0], + SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q2 - 2.0f * q1 * q3, + 2.0f * q0 * q3 + 2.0f * q1 * q2 + }; + + Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][1] * SH_MAG[0] - P[0][2] * SH_MAG[1] + P[0][3] * SH_MAG[2] + P[0][0] * SK_MX[2] - P[0][16] * SK_MX[1] + P[0][17] * SK_MX[4] - P[0][18] * SK_MX[3]); + Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][1] * SH_MAG[0] - P[1][2] * SH_MAG[1] + P[1][3] * SH_MAG[2] + P[1][0] * SK_MX[2] - P[1][16] * SK_MX[1] + P[1][17] * SK_MX[4] - P[1][18] * SK_MX[3]); + Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][1] * SH_MAG[0] - P[2][2] * SH_MAG[1] + P[2][3] * SH_MAG[2] + P[2][0] * SK_MX[2] - P[2][16] * SK_MX[1] + P[2][17] * SK_MX[4] - P[2][18] * SK_MX[3]); + Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][1] * SH_MAG[0] - P[3][2] * SH_MAG[1] + P[3][3] * SH_MAG[2] + P[3][0] * SK_MX[2] - P[3][16] * SK_MX[1] + P[3][17] * SK_MX[4] - P[3][18] * SK_MX[3]); + Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][1] * SH_MAG[0] - P[4][2] * SH_MAG[1] + P[4][3] * SH_MAG[2] + P[4][0] * SK_MX[2] - P[4][16] * SK_MX[1] + P[4][17] * SK_MX[4] - P[4][18] * SK_MX[3]); + Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][1] * SH_MAG[0] - P[5][2] * SH_MAG[1] + P[5][3] * SH_MAG[2] + P[5][0] * SK_MX[2] - P[5][16] * SK_MX[1] + P[5][17] * SK_MX[4] - P[5][18] * SK_MX[3]); + Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][1] * SH_MAG[0] - P[6][2] * SH_MAG[1] + P[6][3] * SH_MAG[2] + P[6][0] * SK_MX[2] - P[6][16] * SK_MX[1] + P[6][17] * SK_MX[4] - P[6][18] * SK_MX[3]); + Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][1] * SH_MAG[0] - P[7][2] * SH_MAG[1] + P[7][3] * SH_MAG[2] + P[7][0] * SK_MX[2] - P[7][16] * SK_MX[1] + P[7][17] * SK_MX[4] - P[7][18] * SK_MX[3]); + Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][1] * SH_MAG[0] - P[8][2] * SH_MAG[1] + P[8][3] * SH_MAG[2] + P[8][0] * SK_MX[2] - P[8][16] * SK_MX[1] + P[8][17] * SK_MX[4] - P[8][18] * SK_MX[3]); + Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][1] * SH_MAG[0] - P[9][2] * SH_MAG[1] + P[9][3] * SH_MAG[2] + P[9][0] * SK_MX[2] - P[9][16] * SK_MX[1] + P[9][17] * SK_MX[4] - P[9][18] * SK_MX[3]); + Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][1] * SH_MAG[0] - P[10][2] * SH_MAG[1] + P[10][3] * SH_MAG[2] + P[10][0] * SK_MX[2] - P[10][16] * SK_MX[1] + P[10][17] * SK_MX[4] - P[10][18] * SK_MX[3]); + Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][1] * SH_MAG[0] - P[11][2] * SH_MAG[1] + P[11][3] * SH_MAG[2] + P[11][0] * SK_MX[2] - P[11][16] * SK_MX[1] + P[11][17] * SK_MX[4] - P[11][18] * SK_MX[3]); + Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][1] * SH_MAG[0] - P[12][2] * SH_MAG[1] + P[12][3] * SH_MAG[2] + P[12][0] * SK_MX[2] - P[12][16] * SK_MX[1] + P[12][17] * SK_MX[4] - P[12][18] * SK_MX[3]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MX[0] * (P[stateIndex][19] + P[stateIndex][1] * SH_MAG[0] - P[stateIndex][2] * SH_MAG[1] + P[stateIndex][3] * SH_MAG[2] + P[stateIndex][0] * SK_MX[2] - P[stateIndex][16] * SK_MX[1] + P[stateIndex][17] * SK_MX[4] - P[stateIndex][18] * SK_MX[3]); + } + + Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][1] * SH_MAG[0] - P[16][2] * SH_MAG[1] + P[16][3] * SH_MAG[2] + P[16][0] * SK_MX[2] - P[16][16] * SK_MX[1] + P[16][17] * SK_MX[4] - P[16][18] * SK_MX[3]); + Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][1] * SH_MAG[0] - P[17][2] * SH_MAG[1] + P[17][3] * SH_MAG[2] + P[17][0] * SK_MX[2] - P[17][16] * SK_MX[1] + P[17][17] * SK_MX[4] - P[17][18] * SK_MX[3]); + Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][1] * SH_MAG[0] - P[18][2] * SH_MAG[1] + P[18][3] * SH_MAG[2] + P[18][0] * SK_MX[2] - P[18][16] * SK_MX[1] + P[18][17] * SK_MX[4] - P[18][18] * SK_MX[3]); + Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][1] * SH_MAG[0] - P[19][2] * SH_MAG[1] + P[19][3] * SH_MAG[2] + P[19][0] * SK_MX[2] - P[19][16] * SK_MX[1] + P[19][17] * SK_MX[4] - P[19][18] * SK_MX[3]); + Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][1] * SH_MAG[0] - P[20][2] * SH_MAG[1] + P[20][3] * SH_MAG[2] + P[20][0] * SK_MX[2] - P[20][16] * SK_MX[1] + P[20][17] * SK_MX[4] - P[20][18] * SK_MX[3]); + Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][1] * SH_MAG[0] - P[21][2] * SH_MAG[1] + P[21][3] * SH_MAG[2] + P[21][0] * SK_MX[2] - P[21][16] * SK_MX[1] + P[21][17] * SK_MX[4] - P[21][18] * SK_MX[3]); + } + else if (obsIndex == 1) + { + for (uint8_t i = 0; i < num_states; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2.0f * magD * q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2.0f * q1 * q2 - 2.0f * q0 * q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2.0f * q0 * q1 + 2.0f * q2 * q3; + H_MAG[19] = 0.0f; + H_MAG[20] = 1.0f; + H_MAG[21] = 0.0f; + + const Vector5 SK_MY{ + 1.0f / varInnovMag[1], + SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q3 - 2.0f * q1 * q2, + 2.0f * q0 * q1 + 2.0f * q2 * q3 + }; + + Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][0] * SH_MAG[2] + P[0][1] * SH_MAG[1] + P[0][2] * SH_MAG[0] - P[0][3] * SK_MY[2] - P[0][17] * SK_MY[1] - P[0][16] * SK_MY[3] + P[0][18] * SK_MY[4]); + Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][0] * SH_MAG[2] + P[1][1] * SH_MAG[1] + P[1][2] * SH_MAG[0] - P[1][3] * SK_MY[2] - P[1][17] * SK_MY[1] - P[1][16] * SK_MY[3] + P[1][18] * SK_MY[4]); + Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][0] * SH_MAG[2] + P[2][1] * SH_MAG[1] + P[2][2] * SH_MAG[0] - P[2][3] * SK_MY[2] - P[2][17] * SK_MY[1] - P[2][16] * SK_MY[3] + P[2][18] * SK_MY[4]); + Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][0] * SH_MAG[2] + P[3][1] * SH_MAG[1] + P[3][2] * SH_MAG[0] - P[3][3] * SK_MY[2] - P[3][17] * SK_MY[1] - P[3][16] * SK_MY[3] + P[3][18] * SK_MY[4]); + Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][0] * SH_MAG[2] + P[4][1] * SH_MAG[1] + P[4][2] * SH_MAG[0] - P[4][3] * SK_MY[2] - P[4][17] * SK_MY[1] - P[4][16] * SK_MY[3] + P[4][18] * SK_MY[4]); + Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][0] * SH_MAG[2] + P[5][1] * SH_MAG[1] + P[5][2] * SH_MAG[0] - P[5][3] * SK_MY[2] - P[5][17] * SK_MY[1] - P[5][16] * SK_MY[3] + P[5][18] * SK_MY[4]); + Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][0] * SH_MAG[2] + P[6][1] * SH_MAG[1] + P[6][2] * SH_MAG[0] - P[6][3] * SK_MY[2] - P[6][17] * SK_MY[1] - P[6][16] * SK_MY[3] + P[6][18] * SK_MY[4]); + Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][0] * SH_MAG[2] + P[7][1] * SH_MAG[1] + P[7][2] * SH_MAG[0] - P[7][3] * SK_MY[2] - P[7][17] * SK_MY[1] - P[7][16] * SK_MY[3] + P[7][18] * SK_MY[4]); + Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][0] * SH_MAG[2] + P[8][1] * SH_MAG[1] + P[8][2] * SH_MAG[0] - P[8][3] * SK_MY[2] - P[8][17] * SK_MY[1] - P[8][16] * SK_MY[3] + P[8][18] * SK_MY[4]); + Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][0] * SH_MAG[2] + P[9][1] * SH_MAG[1] + P[9][2] * SH_MAG[0] - P[9][3] * SK_MY[2] - P[9][17] * SK_MY[1] - P[9][16] * SK_MY[3] + P[9][18] * SK_MY[4]); + Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][0] * SH_MAG[2] + P[10][1] * SH_MAG[1] + P[10][2] * SH_MAG[0] - P[10][3] * SK_MY[2] - P[10][17] * SK_MY[1] - P[10][16] * SK_MY[3] + P[10][18] * SK_MY[4]); + Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][0] * SH_MAG[2] + P[11][1] * SH_MAG[1] + P[11][2] * SH_MAG[0] - P[11][3] * SK_MY[2] - P[11][17] * SK_MY[1] - P[11][16] * SK_MY[3] + P[11][18] * SK_MY[4]); + Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][0] * SH_MAG[2] + P[12][1] * SH_MAG[1] + P[12][2] * SH_MAG[0] - P[12][3] * SK_MY[2] - P[12][17] * SK_MY[1] - P[12][16] * SK_MY[3] + P[12][18] * SK_MY[4]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MY[0] * (P[stateIndex][20] + P[stateIndex][0] * SH_MAG[2] + P[stateIndex][1] * SH_MAG[1] + P[stateIndex][2] * SH_MAG[0] - P[stateIndex][3] * SK_MY[2] - P[stateIndex][17] * SK_MY[1] - P[stateIndex][16] * SK_MY[3] + P[stateIndex][18] * SK_MY[4]); + } + + Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][0] * SH_MAG[2] + P[16][1] * SH_MAG[1] + P[16][2] * SH_MAG[0] - P[16][3] * SK_MY[2] - P[16][17] * SK_MY[1] - P[16][16] * SK_MY[3] + P[16][18] * SK_MY[4]); + Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][0] * SH_MAG[2] + P[17][1] * SH_MAG[1] + P[17][2] * SH_MAG[0] - P[17][3] * SK_MY[2] - P[17][17] * SK_MY[1] - P[17][16] * SK_MY[3] + P[17][18] * SK_MY[4]); + Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][0] * SH_MAG[2] + P[18][1] * SH_MAG[1] + P[18][2] * SH_MAG[0] - P[18][3] * SK_MY[2] - P[18][17] * SK_MY[1] - P[18][16] * SK_MY[3] + P[18][18] * SK_MY[4]); + Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][0] * SH_MAG[2] + P[19][1] * SH_MAG[1] + P[19][2] * SH_MAG[0] - P[19][3] * SK_MY[2] - P[19][17] * SK_MY[1] - P[19][16] * SK_MY[3] + P[19][18] * SK_MY[4]); + Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][0] * SH_MAG[2] + P[20][1] * SH_MAG[1] + P[20][2] * SH_MAG[0] - P[20][3] * SK_MY[2] - P[20][17] * SK_MY[1] - P[20][16] * SK_MY[3] + P[20][18] * SK_MY[4]); + Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][0] * SH_MAG[2] + P[21][1] * SH_MAG[1] + P[21][2] * SH_MAG[0] - P[21][3] * SK_MY[2] - P[21][17] * SK_MY[1] - P[21][16] * SK_MY[3] + P[21][18] * SK_MY[4]); + } + else if (obsIndex == 2) + { + for (uint8_t i = 0; i < num_states; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = -SH_MAG[2]; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2.0f * q0 * q2 + 2.0f * q1 * q3; + H_MAG[17] = 2.0f * q2 * q3 - 2.0f * q0 * q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[19] = 0.0f; + H_MAG[20] = 0.0f; + H_MAG[21] = 1.0f; + + const Vector5 SK_MZ{ + 1.0f / varInnovMag[2], + SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q1 - 2.0f * q2 * q3, + 2.0f * q0 * q2 + 2.0f * q1 * q3 + }; + + Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][0] * SH_MAG[1] - P[0][1] * SH_MAG[2] + P[0][3] * SH_MAG[0] + P[0][2] * SK_MZ[2] + P[0][18] * SK_MZ[1] + P[0][16] * SK_MZ[4] - P[0][17] * SK_MZ[3]); + Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][0] * SH_MAG[1] - P[1][1] * SH_MAG[2] + P[1][3] * SH_MAG[0] + P[1][2] * SK_MZ[2] + P[1][18] * SK_MZ[1] + P[1][16] * SK_MZ[4] - P[1][17] * SK_MZ[3]); + Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][0] * SH_MAG[1] - P[2][1] * SH_MAG[2] + P[2][3] * SH_MAG[0] + P[2][2] * SK_MZ[2] + P[2][18] * SK_MZ[1] + P[2][16] * SK_MZ[4] - P[2][17] * SK_MZ[3]); + Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][0] * SH_MAG[1] - P[3][1] * SH_MAG[2] + P[3][3] * SH_MAG[0] + P[3][2] * SK_MZ[2] + P[3][18] * SK_MZ[1] + P[3][16] * SK_MZ[4] - P[3][17] * SK_MZ[3]); + Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][0] * SH_MAG[1] - P[4][1] * SH_MAG[2] + P[4][3] * SH_MAG[0] + P[4][2] * SK_MZ[2] + P[4][18] * SK_MZ[1] + P[4][16] * SK_MZ[4] - P[4][17] * SK_MZ[3]); + Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][0] * SH_MAG[1] - P[5][1] * SH_MAG[2] + P[5][3] * SH_MAG[0] + P[5][2] * SK_MZ[2] + P[5][18] * SK_MZ[1] + P[5][16] * SK_MZ[4] - P[5][17] * SK_MZ[3]); + Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][0] * SH_MAG[1] - P[6][1] * SH_MAG[2] + P[6][3] * SH_MAG[0] + P[6][2] * SK_MZ[2] + P[6][18] * SK_MZ[1] + P[6][16] * SK_MZ[4] - P[6][17] * SK_MZ[3]); + Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][0] * SH_MAG[1] - P[7][1] * SH_MAG[2] + P[7][3] * SH_MAG[0] + P[7][2] * SK_MZ[2] + P[7][18] * SK_MZ[1] + P[7][16] * SK_MZ[4] - P[7][17] * SK_MZ[3]); + Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][0] * SH_MAG[1] - P[8][1] * SH_MAG[2] + P[8][3] * SH_MAG[0] + P[8][2] * SK_MZ[2] + P[8][18] * SK_MZ[1] + P[8][16] * SK_MZ[4] - P[8][17] * SK_MZ[3]); + Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][0] * SH_MAG[1] - P[9][1] * SH_MAG[2] + P[9][3] * SH_MAG[0] + P[9][2] * SK_MZ[2] + P[9][18] * SK_MZ[1] + P[9][16] * SK_MZ[4] - P[9][17] * SK_MZ[3]); + Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][0] * SH_MAG[1] - P[10][1] * SH_MAG[2] + P[10][3] * SH_MAG[0] + P[10][2] * SK_MZ[2] + P[10][18] * SK_MZ[1] + P[10][16] * SK_MZ[4] - P[10][17] * SK_MZ[3]); + Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][0] * SH_MAG[1] - P[11][1] * SH_MAG[2] + P[11][3] * SH_MAG[0] + P[11][2] * SK_MZ[2] + P[11][18] * SK_MZ[1] + P[11][16] * SK_MZ[4] - P[11][17] * SK_MZ[3]); + Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][0] * SH_MAG[1] - P[12][1] * SH_MAG[2] + P[12][3] * SH_MAG[0] + P[12][2] * SK_MZ[2] + P[12][18] * SK_MZ[1] + P[12][16] * SK_MZ[4] - P[12][17] * SK_MZ[3]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MZ[0] * (P[stateIndex][21] + P[stateIndex][0] * SH_MAG[1] - P[stateIndex][1] * SH_MAG[2] + P[stateIndex][3] * SH_MAG[0] + P[stateIndex][2] * SK_MZ[2] + P[stateIndex][18] * SK_MZ[1] + P[stateIndex][16] * SK_MZ[4] - P[stateIndex][17] * SK_MZ[3]); + } + + Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][0] * SH_MAG[1] - P[16][1] * SH_MAG[2] + P[16][3] * SH_MAG[0] + P[16][2] * SK_MZ[2] + P[16][18] * SK_MZ[1] + P[16][16] * SK_MZ[4] - P[16][17] * SK_MZ[3]); + Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][0] * SH_MAG[1] - P[17][1] * SH_MAG[2] + P[17][3] * SH_MAG[0] + P[17][2] * SK_MZ[2] + P[17][18] * SK_MZ[1] + P[17][16] * SK_MZ[4] - P[17][17] * SK_MZ[3]); + Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][0] * SH_MAG[1] - P[18][1] * SH_MAG[2] + P[18][3] * SH_MAG[0] + P[18][2] * SK_MZ[2] + P[18][18] * SK_MZ[1] + P[18][16] * SK_MZ[4] - P[18][17] * SK_MZ[3]); + Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][0] * SH_MAG[1] - P[19][1] * SH_MAG[2] + P[19][3] * SH_MAG[0] + P[19][2] * SK_MZ[2] + P[19][18] * SK_MZ[1] + P[19][16] * SK_MZ[4] - P[19][17] * SK_MZ[3]); + Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][0] * SH_MAG[1] - P[20][1] * SH_MAG[2] + P[20][3] * SH_MAG[0] + P[20][2] * SK_MZ[2] + P[20][18] * SK_MZ[1] + P[20][16] * SK_MZ[4] - P[20][17] * SK_MZ[3]); + Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][0] * SH_MAG[1] - P[21][1] * SH_MAG[2] + P[21][3] * SH_MAG[0] + P[21][2] * SK_MZ[2] + P[21][18] * SK_MZ[1] + P[21][16] * SK_MZ[4] - P[21][17] * SK_MZ[3]); + } + + for (int i = 0; i < num_states; i++) + { + for (unsigned j = 0; j <= 3; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 4; j <= 15; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 16; j <= 21; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 22; j <= 23; j++) { + KH[i][j] = 0.0f; + } + } + for (int j = 0; j < num_states; j++) + { + for (int i = 0; i < num_states; i++) + { + float res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][16] * P[16][j]; + res += KH[i][17] * P[17][j]; + res += KH[i][18] * P[18][j]; + res += KH[i][19] * P[19][j]; + res += KH[i][20] * P[20][j]; + res += KH[i][21] * P[21][j]; + KHP[i][j] = res; + } + } + bool healthyFusion = true; + for (uint8_t i = 0; i < num_states; i++) + { + if (KHP[i][i] > P[i][i]) + { + healthyFusion = false; + } + } + if (healthyFusion) + { + for (uint8_t i = 0; i < num_states; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + ForceSymmetry(); + ConstrainVariances(); + FillStateArray(); + for (uint8_t j = 0; j < num_states; j++) + { + statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex]; + } + FillStateStruct(); + StateQuat.Normalize(); + quat2arr(); + } + else + { + CovarianceInit(); + return; + } + } +} + +void EKF::VelPosFusion() +{ + if (!readBaro() || !readGPS()) + { + return; + } + //UE_LOG(LogTemp, Display, TEXT("GPS data: %f, %f, %f, %f, %f, %f, %f"), recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt, recentGPS.VelocityEast, recentGPS.VelocityNorth, recentGPS.VelocityUP, recentGPS.dt); + FVector gps_vel(recentGPS.VelocityNorth, recentGPS.VelocityEast, recentGPS.VelocityUP); + CalculateVelInnovationsAndVariances(gps_vel, _gpsHorizVelNoise, gpsNEVelVarAccScale, gpsVelInnov, gpsVelVarInnov); + + velPosObs[0] = recentGPS.VelocityNorth; + velPosObs[1] = recentGPS.VelocityEast; + velPosObs[2] = recentGPS.VelocityUP; + + Location gpsloc; + gpsloc.SetPositionGeodetic(recentGPS.LonDeg, recentGPS.LatDeg, 0); + FVector posxy = EKF_origin.GetDistanceNED(gpsloc); + //UE_LOG(LogTemp, Display, TEXT("PosXY: %f, %f"), posxy.X, posxy.Y) + velPosObs[3] = posxy.X; + velPosObs[4] = posxy.Y; + + selectHeightForFusion(); + + FuseVelPosNED(); +} + +void EKF::CalculateVelInnovationsAndVariances(const FVector& velocity, float noise, float accel_scale, FVector& innovations, FVector& variances) const +{ + innovations = State.velocity - velocity; + //UE_LOG(LogTemp, Display, TEXT("Velocity Innovation: %f, %f, %f"), innovations.X, innovations.Y, innovations.Z); + //const float obs_data_chk = sq(constrain_value(noise, 0.05, 5.0)) + sq(accel_scale * accNavMag); + const float obs_data_chk = sq(constrain_value(noise, 0.05, 5.0)); + variances.X = P[4][4] + obs_data_chk; + variances.Y = P[5][5] + obs_data_chk; + variances.Z = P[6][6] + obs_data_chk; + //UE_LOG(LogTemp, Display, TEXT("Velocity variances: %f, %f, %f"), variances.X, variances.Y, variances.Z); +} + +void EKF::selectHeightForFusion() +{ + readBaro(); + switch (HeightSource) + { + case SensorType::BARO: + posDownObsNoise = sq(constrain_value(_baroAltNoise, 0.1f, 100.0f)); + hgtMea = -recentBaro.altitude; + break; + case SensorType::GPS: + posDownObsNoise = sq(_gpsVertPosNoise); + hgtMea = recentGPS.Alt; + } + velPosObs[5] = hgtMea; +} + +void EKF::FuseVelPosNED() +{ + bool fuseData[6]{}; + uint8_t stateIndex; + uint8_t obsIndex; + + Vector6 R_OBS; + Vector6 R_OBS_DATA_CHECKS; + float SK; + + //R_OBS[0] = sq(constrain_value(gpsSpdAccuracy, _gpsHorizVelNoise, 50.0f)); + R_OBS[0] = sq(_gpsHorizVelNoise); + //R_OBS[2] = sq(constrain_value(gpsSpdAccuracy, _gpsVertVelNoise, 50.0f)); + R_OBS[2] = sq(_gpsVertVelNoise); + R_OBS[1] = R_OBS[0]; + //R_OBS[3] = sq(constrain_value(gpsPosAccuracy, _gpsHorizPosNoise, 100.0f)); + R_OBS[3] = sq(_gpsHorizPosNoise); + R_OBS[4] = R_OBS[3]; + switch (HeightSource) + { + case SensorType::BARO: + R_OBS[5] = sq(_baroAltNoise); + break; + case SensorType::GPS: + R_OBS[5] = sq(_gpsVertPosNoise); + break; + } + //R_OBS[5] = gpsHgtAccuracy; + + //float obs_data_chk; + + //obs_data_chk = sq(constrain_value(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(gpsNEVelVarAccScale * accNavMag); + + //R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk; + + for (uint8_t i = 3; i <= 5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; + + + + //UE_LOG(LogTemp, Warning, TEXT("Velocity before fusion: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.velocity before fusion: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + for (obsIndex = 0; obsIndex <= 5; obsIndex++) + { + stateIndex = 4 + obsIndex; + if (obsIndex <= 2) + { + innovVelPos[obsIndex] = State.velocity[obsIndex] - velPosObs[obsIndex]; + //R_OBS[obsIndex] *= sq(gpsNoiseScaler); + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = State.position[obsIndex - 3] - velPosObs[obsIndex]; + //R_OBS[obsIndex] *= sq(gpsNoiseScaler); + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = State.position[obsIndex - 3] - velPosObs[obsIndex]; + //float gndMaxBaroErr = fmax(_baroGndEffectDeadZone, 0.0); + //float gndBaroInnovFloor = -0.5; + //innovVelPos[5] += constrain_value(-innovVelPos[5] + gndBaroInnovFloor, 0.0f, gndBaroInnovFloor + gndMaxBaroErr); + } + + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0f / varInnovVelPos[obsIndex]; + for (uint8_t i = 0; i <= 21; i++) + { + Kfusion[i] = P[i][stateIndex] * SK; + } + + for (uint8_t i = 0; i < num_states; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < num_states; i++) + { + if (KHP[i][i] > P[i][i]) + { + healthyFusion = false; + } + } + + if (healthyFusion) + { + for (uint8_t i = 0; i < num_states; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + ForceSymmetry(); + ConstrainVariances(); + FillStateArray(); + for (uint8_t i = 0; i < num_states; i++) + { + statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + FillStateStruct(); + StateQuat.Normalize(); + quat2arr(); + } + } + //UE_LOG(LogTemp, Warning, TEXT("Velocity after fusion: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.elocity: %f, %f, %f!"),State.velocity.X, State.velocity.Y, State.velocity.Z) +} + +void EKF::fuseEulerYaw() +{ + double q0 = StateQuat(0); + double q1 = StateQuat(1); + double q2 = StateQuat(2); + double q3 = StateQuat(3); + + float yawAngPredicted; + float H_YAW[4]; + Matrix33 Tbn_zeroYaw; + + bool canUseA = false; + float SA0 = 2 * q3; + float SA1 = 2 * q2; + float SA2 = SA0 * q0 + SA1 * q1; + float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + float SA4, SA5_inv; + if (is_positive(sq(SA3))) + { + SA4 = 1.0F / sq(SA3); + SA5_inv = sq(SA2) * SA4 + 1; + canUseA = is_positive(fabsf(SA5_inv)); + } + + bool canUseB = false; + float SB0 = 2 * q0; + float SB1 = 2 * q1; + float SB2 = SB0 * q3 + SB1 * q2; + float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + float SB3, SB5_inv; + if (is_positive(sq(SB2))) + { + SB3 = 1.0F / sq(SB2); + SB5_inv = SB3 * sq(SB4) + 1; + canUseB = is_positive(fabsf(SB5_inv)); + } + + if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { + float SA5 = 1.0F / SA5_inv; + float SA6 = 1.0F / (SA3); + float SA7 = SA2 * SA4; + float SA8 = 2 * SA7; + float SA9 = 2 * SA6; + + H_YAW[0] = SA5 * (SA0 * SA6 - SA8 * q0); + H_YAW[1] = SA5 * (SA1 * SA6 - SA8 * q1); + H_YAW[2] = SA5 * (SA1 * SA7 + SA9 * q1); + H_YAW[3] = SA5 * (SA0 * SA7 + SA9 * q0); + } + else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) + { + float SB5 = 1.0F / SB5_inv; + float SB6 = 1.0F / (SB2); + float SB7 = SB3 * SB4; + float SB8 = 2 * SB7; + float SB9 = 2 * SB6; + + H_YAW[0] = -SB5 * (SB0 * SB6 - SB8 * q3); + H_YAW[1] = -SB5 * (SB1 * SB6 - SB8 * q2); + H_YAW[2] = -SB5 * (-SB1 * SB7 - SB9 * q2); + H_YAW[3] = -SB5 * (-SB0 * SB7 - SB9 * q3); + } + else + { + return; + } + + FVector euler321 = StateQuat.GetEulerRad(); + yawAngPredicted = euler321.Z; + FQuaternion q_zer0_yaw; + q_zer0_yaw.InitializeFromEulerAngles(euler321.X, euler321.Y, 0.0f); + Tbn_zeroYaw = q_zer0_yaw.GetMatrix(); + + FVector magMeasNED = Tbn_zeroYaw * recentMag.data; + float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.Y, magMeasNED.X) + MF_DECLINATION); + //UE_LOG(LogTemp, Display, TEXT("Yaw measured %f"), yawAngMeasured); + innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured); + //UE_LOG(LogTemp, Display, TEXT("Yaw angle innovation %f"), innovYaw); + + float R_YAW = sq(_yawNoise); + float PH[4]; + float varInnov = R_YAW; + for (uint8_t rowIndex = 0; rowIndex <= 3; rowIndex++) + { + PH[rowIndex] = 0.0f; + for (uint8_t colIndex = 0; colIndex <= 3; colIndex++) + { + PH[rowIndex] += P[rowIndex][colIndex] * H_YAW[colIndex]; + } + varInnov += H_YAW[rowIndex] * PH[rowIndex]; + } + float varInnovInv; + if (varInnov >= R_YAW) + { + varInnovInv = 1.0f / varInnov; + } + else + { + CovarianceInit(); + return; + } + for (uint8_t rowIndex = 0; rowIndex < num_states; rowIndex++) + { + Kfusion[rowIndex] = 0.0f; + for (uint8_t colIndex = 0; colIndex <= 3; colIndex++) + { + Kfusion[rowIndex] += P[rowIndex][colIndex] * H_YAW[colIndex]; + //UE_LOG(LogTemp, Display, TEXT("P[%d][%d] %f H_YAW[%d] %f"), rowIndex, colIndex, P[rowIndex][colIndex], colIndex, H_YAW[colIndex]); + } + Kfusion[rowIndex] *= varInnovInv; + } + for (uint8_t row = 0; row < num_states; row++) + { + for (uint8_t column = 0; column <= 3; column++) + { + KH[row][column] = Kfusion[row] * H_YAW[column]; + } + } + for (uint8_t row = 0; row <= stateIndexLim; row++) + { + for (uint8_t column = 0; column <= stateIndexLim; column++) + { + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; + KHP[row][column] = tmp; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < num_states; i++) + { + if (KHP[i][i] > P[i][i]) + { + healthyFusion = false; + } + } + if (healthyFusion) + { + for (uint8_t i = 0; i < num_states; i++) + { + for (uint8_t j = 0; j < num_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + ForceSymmetry(); + ConstrainVariances(); + + FillStateArray(); + for (uint8_t i = 0; i < num_states; i++) + { + //UE_LOG(LogTemp, Display, TEXT("Yaw angle Kfusion[%d] %f"), i, Kfusion[i]); + statesArray[i] -= Kfusion[i] * constrain_value(innovYaw, -0.5f, 0.5f); + } + FillStateStruct(); + StateQuat.Normalize(); + quat2arr(); + } +} diff --git a/Source/LuckyWorld/EKF.h b/Source/LuckyWorld/EKF.h new file mode 100644 index 00000000..628a2938 --- /dev/null +++ b/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& 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* GyroDataBuffer; + TArray* AccelDataBuffer; + TArray* MagDataBuffer; + TArray* GPSDataBuffer; + TArray* 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 GyroReadings; + TArray AccelReadings; + TArray MagReadings; + TArray GPSreadings; + TArray 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* sensor_buffer, double* data_array, int num); + bool readMagData(); + bool readGPS(); + bool readBaro(); + float GetMagYawAngle(); + + //template + //bool readNewData(TArray& 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(fval) >= FLT_EPSILON); + } +}; diff --git a/Source/LuckyWorld/EKFilter.cpp b/Source/LuckyWorld/EKFilter.cpp new file mode 100644 index 00000000..05987677 --- /dev/null +++ b/Source/LuckyWorld/EKFilter.cpp @@ -0,0 +1,2205 @@ +// Fill out your copyright notice in the Description page of Project Settings. + + +#include "EKFilter.h" + +EKFilter::EKFilter() +{ +} + +EKFilter::~EKFilter() +{ +} + +void EKFilter::FillStateStruct() +{ + arr2quat(); + State.velocity = FVector(StateArray[4], StateArray[5], StateArray[6]); + State.position = FVector(StateArray[7], StateArray[8], StateArray[9]); + State.gyro_bias = FVector(StateArray[10], StateArray[11], StateArray[12]); + State.accel_bias = FVector(StateArray[13], StateArray[14], StateArray[15]); + State.earth_magfield = FVector(StateArray[16], StateArray[17], StateArray[18]); + State.body_magfield = FVector(StateArray[19], StateArray[20], StateArray[21]); +} + +void EKFilter::FillStateArray() +{ + quat2arr(); + + StateArray[4] = State.velocity.X; + StateArray[5] = State.velocity.Y; + StateArray[6] = State.velocity.Z; + + StateArray[7] = State.position.X; + StateArray[8] = State.position.Y; + StateArray[9] = State.position.Z; + + StateArray[10] = State.gyro_bias.X; + StateArray[11] = State.gyro_bias.Y; + StateArray[12] = State.gyro_bias.Z; + + StateArray[13] = State.accel_bias.X; + StateArray[14] = State.accel_bias.Y; + StateArray[15] = State.accel_bias.Z; + + StateArray[16] = State.earth_magfield.X; + StateArray[17] = State.earth_magfield.Y; + StateArray[18] = State.earth_magfield.Z; + + StateArray[19] = State.body_magfield.X; + StateArray[20] = State.body_magfield.Y; + StateArray[21] = State.body_magfield.Z; +} + +void EKFilter::RegisterBuffer(Sensor sensor_type, TArray& buffer) +{ + switch (sensor_type) + { + case Sensor::GYRO: + GyroDataBuffer = &buffer; + GyroBufferSet = true; + break; + case Sensor::ACCEL: + AccelDataBuffer = &buffer; + AccelBufferSet = true; + break; + case Sensor::MAG: + MagDataBuffer = &buffer; + MagBufferSet = true; + break; + case Sensor::GPS: + GPSDataBuffer = &buffer; + GPSBufferSet = true; + break; + case Sensor::BARO: + BaroDataBuffer = &buffer; + BaroBufferSet = true; + break; + default: + break; + } +} + +bool EKFilter::GetNextData(TArray* sensor_buffer, double* data_array, int num) +{ + if (sensor_buffer && sensor_buffer->Num() < num) + { + return false; + } + int count = 0; + while (count < num) + { + data_array[count] = (*sensor_buffer)[0]; + sensor_buffer->RemoveAt(0); + count++; + } + return true; +} + +bool EKFilter::readIMUData() +{ + if (!GyroBufferSet || !AccelBufferSet) + { + UE_LOG(LogTemp, Warning, TEXT("NO IMU BUFFER!")) + return false; + } + + int32 num_dp = 0; + FVector gyro_data = { 0, 0, 0 }; + float dt = 0; + double imu_data[4]; + while (GetNextData(GyroDataBuffer, imu_data, 4)) + { + FVector gyro(imu_data[0], imu_data[1], imu_data[2]); + gyro_data += gyro; + dt += imu_data[3]; + num_dp++; + } + + if (num_dp == 0) + { + return false; + } + FVector prev_gyro = recentGyro.data; + recentGyro.data = gyro_data / num_dp; + recentGyro.prev_data = prev_gyro; + recentGyro.dt = dt / num_dp; + recentGyro.data_dt = 0.5 * (recentGyro.data + recentGyro.prev_data) * recentGyro.dt; + recentGyro.data_dt -= State.gyro_bias * recentGyro.dt; + recentGyro.data_corrected = recentGyro.data_dt - (1 / 12) * FVector::CrossProduct(recentGyro.prev_data_dt, recentGyro.data_dt); + //UE_LOG(LogTemp, Display, TEXT("Recent gyro %f %f %f"), recentGyro.data_corrected.X, recentGyro.data_corrected.Y, recentGyro.data_corrected.Z); + recentGyro.prev_data_dt = recentGyro.data_dt; + + num_dp = 0; + dt = 0; + FVector accel_data = { 0, 0, 0 }; + while (GetNextData(AccelDataBuffer, imu_data, 4)) + { + FVector accel(imu_data[0], imu_data[1], imu_data[2]); + accel_data += accel; + dt += imu_data[3]; + num_dp++; + } + if (num_dp == 0) + { + return false; + } + FVector prev_accel = recentAccel.data; + recentAccel.data = accel_data / num_dp; + recentAccel.dt = dt / num_dp; + recentAccel.prev_data = prev_accel; + recentAccel.data_dt = 0.5 * (recentAccel.data + recentAccel.prev_data) * recentAccel.dt; + recentAccel.data_dt -= State.accel_bias * recentAccel.dt; + FVector v1 = FVector::CrossProduct(recentGyro.prev_data_dt + recentGyro.data_dt, recentAccel.prev_data_dt + recentAccel.data_dt); + FVector v2 = FVector::CrossProduct(recentGyro.prev_data_dt + recentGyro.data_dt, v1); + FVector v3 = FVector::CrossProduct(recentGyro.prev_data_dt, recentAccel.data_dt); + FVector v4 = FVector::CrossProduct(recentAccel.prev_data_dt, recentGyro.data_dt); + recentAccel.data_corrected = recentAccel.data_dt + 0.5 * v1 + (1/6) * v2 + (1/12) * v3 + v4; + //UE_LOG(LogTemp, Display, TEXT("Recent accle %f %f %f"), recentAccel.data_corrected.X, recentAccel.data_corrected.Y, recentAccel.data_corrected.Z); + recentAccel.prev_data_dt = recentAccel.data_dt; + + return true; +} + +bool EKFilter::readMagData() +{ + if (!MagBufferSet) + { + UE_LOG(LogTemp, Warning, TEXT("NO MAGNETOMETER BUFFER!")) + return false; + } + + int32 num_dp = 0; + FVector mag_data = { 0, 0, 0 }; + float dt = 0; + double imu_data[4]; + while (GetNextData(MagDataBuffer, imu_data, 4)) + { + FVector mag(imu_data[0], imu_data[1], imu_data[2]); + mag_data += mag; + dt += imu_data[3]; + num_dp++; + } + if (num_dp == 0) + { + return false; + } + recentMag.data = mag_data / num_dp; + recentMag.dt = dt / num_dp; + + return true; +} + +bool EKFilter::readGPS() +{ + + if (!GPSBufferSet) + { + UE_LOG(LogTemp, Warning, TEXT("NO GPS BUFFER!")) + return false; + } + + double gps[7]; + + if (GetNextData(GPSDataBuffer, gps, 7)) + { + recentGPS.LonDeg = gps[0]; + recentGPS.LatDeg = gps[1]; + recentGPS.Alt = gps[2]; + recentGPS.VelocityEast = gps[3]; + recentGPS.VelocityNorth = gps[4]; + recentGPS.VelocityUP = gps[5]; + recentGPS.dt = gps[6]; + //UE_LOG(LogTemp, Warning, TEXT("Recent GPS: %f, %f, %f, %f, %f, %f"), recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt, recentGPS.VelocityEast, recentGPS.VelocityNorth, recentGPS.VelocityUP); + return true; + } + + return false; +} + +bool EKFilter::readBaro() +{ + //return readNewData(BaroReadings, recentBaro); + if (!BaroBufferSet) + { + UE_LOG(LogTemp, Warning, TEXT("NO BARO BUFFER!")) + return false; + } + double baro[3]; + if (GetNextData(BaroDataBuffer, baro, 3)) + { + recentBaro.pressure = baro[0]; + recentBaro.altitude = baro[1]; + recentBaro.dt = baro[2]; + //UE_LOG(LogTemp, Warning, TEXT("Recent Baro: %f, %f!"), recentBaro.pressure, recentBaro.altitude); + return true; + } + return false; +} + +void EKFilter::GetState(StateElements& state_struct) +{ + state_struct.quat = State.quat; + state_struct.position = State.position; + state_struct.velocity = State.velocity; + state_struct.gyro_bias = State.gyro_bias; + state_struct.accel_bias = State.accel_bias; + state_struct.earth_magfield = State.earth_magfield; + state_struct.body_magfield = State.body_magfield; +} + +bool EKFilter::InitialiseFilter() +{ + if (!readGPS() || !readBaro() || !readIMUData() || !readMagData()) + { + return false; + } + + Origin.SetPositionGeodetic(recentGPS.LonDeg, recentGPS.LatDeg, recentGPS.Alt); + + FVector initAccVec; + initAccVec = recentAccel.data_corrected / recentAccel.dt; + float pitch = 0, roll = 0; + if (initAccVec.Size() > 0.001f) + { + initAccVec.Normalize(); + pitch = atan2f(initAccVec.X, -initAccVec.Z); + roll = -atan2f(initAccVec.Y, -initAccVec.Z); + } + + FVector initMag = recentMag.data; + FQuaternion rp; + rp.InitializeFromEulerAngles(roll, pitch, 0.0); + //Matrix33 init_mat = rp.GetMatrix(); + Matrix33 init_mat; + init_mat.FromEuler(roll, pitch, 0.0); + FVector initMagNED = init_mat * initMag; + State.earth_magfield.X = initMagNED.X; + State.earth_magfield.Y = initMagNED.Y; + State.earth_magfield.Z = initMagNED.Z; + + MagFieldDeclination = atan2f(State.earth_magfield.Y, State.earth_magfield.X); + + float yaw = GetMagYawAngle(); + + State.quat.InitializeFromEulerAngles(roll, pitch, yaw); + + State.gyro_bias = { 0.349, 0.349, 0.349 }; + State.accel_bias = { 0.05, 0.05, 0.08 }; + State.body_magfield = { 7.5e-07, 7.5e-07, 7.5e-07 }; + + FillStateArray(); + + InitialzeVariables(); + CovarianceInit(); + + Initialised = true; + return true; +} + +void EKFilter::InitialzeVariables() +{ + memset(&P[0][0], 0, sizeof(P)); + memset(&KH[0][0], 0, sizeof(KH)); + memset(&KHP[0][0], 0, sizeof(KHP)); + memset(&nextP[0][0], 0, sizeof(nextP)); + + //_gyroBiasProcessNoise = 0.5; + //_accelBiasProcessNoise = 0.5; + _gyroBiasProcessNoise = 1e-7; + _accelBiasProcessNoise = 1e-7; + _accBiasLim = 1.0f; + + _magEarthProcessNoise = 1.0E-04f; + _magBodyProcessNoise = 1.0E-04f; + //_magEarthProcessNoise = 1.0e-7f; + //_magBodyProcessNoise = 1.0e-7f; + _magNoise = 1e-6f; + //_magNoise = 0.1f; + + _gyrNoise = 1.5E-04f; + _accNoise = 3.5E-04f; + //_gyrNoise = 0.5f; + //_accNoise = 1.0f; + + //_gpsVertVelNoise = 0.7; + //_gpsHorizVelNoise = 0.5; + //_gpsHorizPosNoise = 3.0; + //_gpsVertPosNoise = 3.0; + _gpsVertVelNoise = 0.5f; + _gpsHorizVelNoise = 0.5f; + _gpsHorizPosNoise = 1.0f; + _gpsVertPosNoise = 1.0f; + + _baroAltNoise = 2.0f; + + _gpsPosInnovGate = 500; + //_gpsVelInnovGate = 500; + _gpsVelInnovGate = 10; + _hgtInnovGate = 500; + _gpsGlitchRadiusMax = 5; + + _yawNoise = 5e-5f; + //_yawNoise = 0.5; + _yawInnovGate = 300; + _magInnovGate = 300; +} +/* +void EKFilter::GetAccelGain(FVector delVel) +{ + float accelFiltRatio{ 10.0 }; + float tiltGain{ 0.2 }; + //FVector ahrsAccel = { 0.0, 0.0, -GRAVITY }; + float filter_coef = fminf(accelFiltRatio * recentAccel.dt * tiltGain, 1.0f); + FVector accel = delVel / fmaxf(recentAccel.dt, 0.001f); + //UE_LOG(LogTemp, Warning, TEXT("Accel date: %f, %f, %f!"), recentAccel.data_corrected.X, recentAccel.data_corrected.Y, recentAccel.data_corrected.Z) + //ahrsAccel = ahrsAccel * (1.0f - filter_coef) + accel * filter_coef; + ahrsAccel = accel; + UE_LOG(LogTemp, Warning, TEXT("AHRS Accel filtered: %f, %f, %f!"), ahrsAccel.X, ahrsAccel.Y, ahrsAccel.Z) + + //float ahrs_accel_norm = accel.Size(); + float ahrs_accel_norm = ahrsAccel.Size(); + float ahrs_ng = ahrs_accel_norm / GRAVITY; + UE_LOG(LogTemp, Display, TEXT("Current ahrs_ng %f"), ahrs_ng); + ahrs_ng = fmax(0.5, fmin(1.5, ahrs_ng)); + if (ahrs_ng > 1.0f) + { + accel_gain = sq(3.0f - 2.0f * ahrs_ng); + } + else + { + accel_gain = 2.0f * ahrs_ng - 1.0f; + } + //accel_gain = fmax(0, fmin(accel_gain, 1.0)); + + /* + if (ahrs_ng > 1.0f) + { + if (accel_gain <= 1.5f) + { + accel_gain = tiltGain * sq(3.0f - 2.0f * ahrs_ng); + } + else + { + accel_gain = 0.0f; + } + } + else if (accel_gain > 0.5) + { + accel_gain = tiltGain * sq(2.0f * ahrs_ng - 1.0f); + } + else + { + accel_gain = 0.0f; + } + */ + //UE_LOG(LogTemp, Display, TEXT("Current accel gain %f"), accel_gain); + //UE_LOG(LogTemp, Display, TEXT("Current accel norm %f"), ahrs_ng); +//} + +void EKFilter::UpdateState() +{ + FVector delAngles = recentGyro.data_corrected; + delAngles.X = fabs(delAngles.X) > 1e-3 ? delAngles.X : 0.0; + delAngles.Y = fabs(delAngles.Y) > 1e-3 ? delAngles.Y : 0.0; + delAngles.Z = fabs(delAngles.Z) > 1e-3 ? delAngles.Z : 0.0; + //UE_LOG(LogTemp, Warning, TEXT("Using delta angles: %f % f %f!"), delAngles.X, delAngles.Y, delAngles.Z); + FQuaternion dq = FQuaternion::from_axis_angle(delAngles); + FQuaternion nq = State.quat * dq; + + float nq_roll = nq.GetRoll(); + float nq_pitch = nq.GetPitch(); + float nq_yaw = nq.GetYaw(); + FVector nq_euler(nq_roll, nq_pitch, nq_yaw); + + FVector bodyAccel = recentAccel.data - State.accel_bias; + double accel_pitch = atan2f(bodyAccel.X, -bodyAccel.Z); + double accel_roll = -atan2f(bodyAccel.Y, -bodyAccel.Z); + FVector accel_euler(accel_roll, accel_pitch, nq_yaw); + + float dt = 0.5 * (recentGyro.dt + recentAccel.dt); + float tau = 0.2; + float gyro_gain = tau / (tau + dt); + float acc_gain = dt / (tau + dt); + FVector EulerNext = nq_euler * gyro_gain + accel_euler * acc_gain; + + //State.quat.InitializeFromEulerAngles(EulerNext.X, EulerNext.Y, EulerNext.Z); + State.quat = nq; + State.quat.Normalize(); + quat2arr(); + + prevTnb = State.quat.GetMatrix(); + + FVector correctedDelVel = recentAccel.data_corrected; + FVector gravity(0, 0, GRAVITY); + FVector delVelNav = prevTnb * correctedDelVel + gravity * recentAccel.dt; + + velDotNED = delVelNav / recentAccel.dt; + + // apply a first order lowpass filter + velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f; + + // calculate a magnitude of the filtered nav acceleration (required for GPS + // variance estimation) + accNavMag = velDotNEDfilt.Size(); + accNavMagHoriz = velDotNEDfilt.Size2D(); + + FVector lastVelocity = State.velocity; + State.velocity += delVelNav; + FVector next_vel = State.velocity + lastVelocity; + //UE_LOG(LogTemp, Warning, TEXT("Using predicted velocity: %f % f %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z); + State.position += (State.velocity + lastVelocity) * (recentAccel.dt * 0.5f); + //UE_LOG(LogTemp, Warning, TEXT("Using predicted state: %f % f %f!"), State.position.X, State.position.Y, State.position.Z); + FillStateArray(); +} + +void EKFilter::setYawFromMag() +{ + float yawAngMeas = GetMagYawAngle(); + FQuaternion quatBeforeReset = State.quat; + FVector Euler = State.quat.GetEulerRad(); + State.quat.InitializeFromEulerAngles(Euler.X, Euler.Y, yawAngMeas); + quat2arr(); + prevTnb = State.quat.Inverse().GetMatrix(); + float deltaYaw = wrap_PI(yawAngMeas - Euler.Z); + FVector angleErrVarVec = FVector(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, 1.0e-2f); + CovariancePrediction(&angleErrVarVec); +} + +float EKFilter::MagDeclination() +{ + float earth_decl = atan2f(State.earth_magfield.Y, State.earth_magfield.X); + return earth_decl; +} + +float EKFilter::GetMagYawAngle() +{ + FVector Euler = State.quat.GetEulerRad(); + Matrix33 Tbn_zeroYaw; + Tbn_zeroYaw.FromEuler(Euler.X, Euler.Y, 0.0f); + FVector magMeasNED = Tbn_zeroYaw * recentMag.data; + float yawAngMeasured = wrap_PI (- atan2(magMeasNED.Y, magMeasNED.X) + MagFieldDeclination); + return yawAngMeasured; +} + +void EKFilter::MagFusion() +{ + if (!readMagData()) + { + return; + } + + //fuseEulerYaw(); + FuseDeclination(0.34f); + FuseMagnetometer(); +} + +void EKFilter::FuseDeclination(float declErr) +{ + float r_decl = pow(declErr, 2); + + float magN = State.earth_magfield.X; + float magE = State.earth_magfield.Y; + + + if (magN < 1e-3f) + { + return; + } + + float magEsq = magE * magE; + float magNsq = magN * magN; + + float t1 = magEsq + magNsq; + + if (fabsf(t1) <= 1e-6f) + { + return; + } + + float H_DECL[22] = {}; + H_DECL[16] = -magE / t1; + H_DECL[17] = -magN / t1; + + float t2 = P[16][16] * magEsq + P[17][17] * magNsq + r_decl * magEsq * magEsq + r_decl * magNsq * magNsq + r_decl * magEsq * magNsq * 2.0f - P[16][17] * magE * magN - P[17][16] * magE * magN; + float t3; + if (fabsf(t2) > 1e-6f) + { + t3 = 1 / t2; + } + else + { + return; + } + + float t5 = t1 * t3; + + for (int i = 0; i < NumStates; i++) + { + Kfusion[i] = -t5 * (P[i][16] * magE - P[i][17] * magN); + } + //float decl = MagDeclination(); + + float innovation = -atan2f(magE, magN) - MagFieldDeclination; + + if (innovation > 0.5f) + { + innovation = 0.5f; + } + else if (innovation < -0.5f) + { + innovation = -0.5f; + } + + for (int i = 0; i < NumStates; i++) + { + for (int j = 0; j <= 15; j++) + { + KH[i][j] = 0.0f; + } + KH[i][16] = Kfusion[i] * H_DECL[16]; + KH[i][17] = Kfusion[i] * H_DECL[17]; + for (int j = 18; j < NumStates; j++) + { + KH[i][j] = 0.0f; + } + } + + for (int j = 0; j < NumStates; j++) + { + for (int i = 0; i < NumStates; i++) + { + KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j]; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < NumStates; i++) + { + if (KHP[i][i] > P[i][i]) + { + //UE_LOG(LogTemp, Display, TEXT("Magnetic field decl fusion KHP[%d][%d], P[%d][%d] : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]); + healthyFusion = false; + } + } + + if (healthyFusion) + { + for (uint8_t i = 0; i < NumStates; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + FillStateArray(); + for (uint8_t j = 0; j < NumStates; j++) + { + StateArray[j] = StateArray[j] - Kfusion[j] * innovation; + } + FillStateStruct(); + State.quat.Normalize(); + quat2arr(); + + ForceSymmetry(); + ConstrainVariances(); + } +} + +void EKFilter::fuseEulerYaw() +{ + FillStateArray(); + + double q0 = State.quat(0); + double q1 = State.quat(1); + double q2 = State.quat(2); + double q3 = State.quat(3); + + float yawAngPredicted; + float H_YAW[4]; + Matrix33 Tbn_zeroYaw; + + bool canUseA = false; + const float SA0 = 2 * q3; + const float SA1 = 2 * q2; + const float SA2 = SA0 * q0 + SA1 * q1; + const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + float SA4, SA5_inv; + if (is_positive(sq(SA3))) + { + SA4 = 1.0F / sq(SA3); + SA5_inv = sq(SA2) * SA4 + 1; + canUseA = is_positive(fabsf(SA5_inv)); + } + + bool canUseB = false; + const float SB0 = 2 * q0; + const float SB1 = 2 * q1; + const float SB2 = SB0 * q3 + SB1 * q2; + const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + float SB3, SB5_inv; + if (is_positive(sq(SB2))) + { + SB3 = 1.0F / sq(SB2); + SB5_inv = SB3 * sq(SB4) + 1; + canUseB = is_positive(fabsf(SB5_inv)); + } + + if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) + { + const float SA5 = 1.0F / SA5_inv; + const float SA6 = 1.0F / (SA3); + const float SA7 = SA2 * SA4; + const float SA8 = 2 * SA7; + const float SA9 = 2 * SA6; + + H_YAW[0] = SA5 * (SA0 * SA6 - SA8 * q0); + H_YAW[1] = SA5 * (SA1 * SA6 - SA8 * q1); + H_YAW[2] = SA5 * (SA1 * SA7 + SA9 * q1); + H_YAW[3] = SA5 * (SA0 * SA7 + SA9 * q0); + } + else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) + { + const float SB5 = 1.0F / SB5_inv; + const float SB6 = 1.0F / (SB2); + const float SB7 = SB3 * SB4; + const float SB8 = 2 * SB7; + const float SB9 = 2 * SB6; + + H_YAW[0] = -SB5 * (SB0 * SB6 - SB8 * q3); + H_YAW[1] = -SB5 * (SB1 * SB6 - SB8 * q2); + H_YAW[2] = -SB5 * (-SB1 * SB7 - SB9 * q2); + H_YAW[3] = -SB5 * (-SB0 * SB7 - SB9 * q3); + } + else + { + return; + } + + float roll = State.quat.GetRoll(); + float pitch = State.quat.GetPitch(); + float yaw = State.quat.GetYaw(); + yawAngPredicted = yaw; + Tbn_zeroYaw.FromEuler(roll, pitch, 0.0); + FVector magMeasNED = Tbn_zeroYaw * recentMag.data; + float yawAngMeasured = wrap_PI(-atan2(magMeasNED.Y, magMeasNED.X) + MagFieldDeclination); + //UE_LOG(LogTemp, Display, TEXT("Yaw measured %f"), yawAngMeasured); + //UE_LOG(LogTemp, Display, TEXT("Yaw predicted %f"), yawAngPredicted); + float innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured); + //UE_LOG(LogTemp, Display, TEXT("Yaw angle innovation %f"), innovYaw); + + float R_YAW = sq(_yawNoise); + float PH[4]; + float varInnov = R_YAW; + for (uint8_t rowIndex = 0; rowIndex <= 3; rowIndex++) + { + PH[rowIndex] = 0.0f; + for (uint8_t colIndex = 0; colIndex <= 3; colIndex++) + { + PH[rowIndex] += P[rowIndex][colIndex] * H_YAW[colIndex]; + } + varInnov += H_YAW[rowIndex] * PH[rowIndex]; + } + float varInnovInv; + if (varInnov >= R_YAW) + { + varInnovInv = 1.0f / varInnov; + } + else + { + CovarianceInit(); + return; + } + + for (uint8_t rowIndex = 0; rowIndex < NumStates; rowIndex++) + { + Kfusion[rowIndex] = 0.0f; + for (uint8_t colIndex = 0; colIndex <= 3; colIndex++) + { + Kfusion[rowIndex] += P[rowIndex][colIndex] * H_YAW[colIndex]; + } + Kfusion[rowIndex] *= varInnovInv; + } + + float yawTestRatio = sq(innovYaw) / (sq(fmax(0.01f * _yawInnovGate, 1.0f)) * varInnov); + ///UE_LOG(LogTemp, Warning, TEXT("YAW TEST RATIO : %f"), yawTestRatio); + if (yawTestRatio > 1.0f) + { + //UE_LOG(LogTemp, Warning, TEXT("BAD MAGNETOMETER HEALTH : %f"), yawTestRatio); + return; + } + + for (uint8_t row = 0; row < NumStates; row++) + { + for (uint8_t column = 0; column <= 3; column++) + { + KH[row][column] = Kfusion[row] * H_YAW[column]; + } + } + + for (uint8_t row = 0; row < NumStates; row++) + { + for (uint8_t column = 0; column < NumStates; column++) + { + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; + KHP[row][column] = tmp; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < NumStates; i++) + { + if (KHP[i][i] > P[i][i]) + { + if (abs(KHP[i][i]) < 1e-6) + { + KHP[i][i] = 0.0; + } + else + { + //UE_LOG(LogTemp, Display, TEXT("Yaw fusion KHP[%d][%d], P[%d][%d] : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]); + healthyFusion = false; + } + } + } + if (healthyFusion) + { + for (uint8_t i = 0; i < NumStates; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + ForceSymmetry(); + ConstrainVariances(); + + FillStateArray(); + for (uint8_t i = 0; i < NumStates; i++) + { + StateArray[i] -= Kfusion[i] * constrain_value(innovYaw, -0.5f, 0.5f); + } + FillStateStruct(); + State.quat.Normalize(); + quat2arr(); + } +} + +void EKFilter::FuseMagnetometer() +{ + const float q0 = State.quat(0); + const float q1 = State.quat(1); + const float q2 = State.quat(2); + const float q3 = State.quat(3); + const float magN = State.earth_magfield.X; + const float magE = State.earth_magfield.Y; + const float magD = State.earth_magfield.Z; + const float magXbias = State.body_magfield.X; + const float magYbias = State.body_magfield.Y; + const float magZbias = State.body_magfield.Z; + + //Matrix33 DCM = State.quat.GetMatrix().Transposed(); + + Matrix33 DCM{ + q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3, + 2.0f * (q1 * q2 + q0 * q3), + 2.0f * (q1 * q3 - q0 * q2), + 2.0f * (q1 * q2 - q0 * q3), + q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3, + 2.0f * (q2 * q3 + q0 * q1), + 2.0f * (q1 * q3 + q0 * q2), + 2.0f * (q2 * q3 - q0 * q1), + q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 + }; + + FVector MagPred = {DCM(0,0) * magN + DCM(0,1) * magE + DCM(0,2) * magD + magXbias, + DCM(1,0) * magN + DCM(1,1) * magE + DCM(1,2) * magD + magYbias, + DCM(2,0) * magN + DCM(2,1) * magE + DCM(2,2) * magD + magZbias}; + + float roll = State.quat.GetRoll(); + float pitch = State.quat.GetPitch(); + float yaw = State.quat.GetYaw(); + DCM.FromEuler(roll, pitch, yaw); + MagPred = DCM.Transposed() * State.earth_magfield; + MagPred.X += magXbias; + MagPred.Y += magYbias; + MagPred.Z += magZbias; + //UE_LOG(LogTemp, Display, TEXT("Earth magnetic field %f %f %f"), State.earth_magfield.X, State.earth_magfield.Y, State.earth_magfield.Z); + //UE_LOG(LogTemp, Display, TEXT("Magnetometer field predicted %f %f %f"), MagPred.X, MagPred.Y, MagPred.Z); + //UE_LOG(LogTemp, Display, TEXT("Magnetometer field measured %f %f %f"), recentMag.data.X, recentMag.data.Y, recentMag.data.Z); + innovMag = MagPred - recentMag.data; + + double R_MAG = sq(constrain_value(_magNoise, 0.01f, 0.5f)) + sq(magVarRateScale * recentGyro.data_corrected.Size() / recentGyro.dt); + //const float R_MAG = _magNoise; + const Vector9 SH_MAG{ + 2.0f * magD * q3 + 2.0f * magE * q2 + 2.0f * magN * q1, + 2.0f * magD * q0 - 2.0f * magE * q1 + 2.0f * magN * q2, + 2.0f * magD * q1 + 2.0f * magE * q0 - 2.0f * magN * q3, + sq(q3), + sq(q2), + sq(q1), + sq(q0), + 2.0f * magN * q0, + 2.0f * magE * q3 + }; + + varInnovMag.X = (P[19][19] + R_MAG + P[1][19] * SH_MAG[0] - P[2][19] * SH_MAG[1] + P[3][19] * SH_MAG[2] - P[16][19] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f * q0 * q3 + 2.0f * q1 * q2) * (P[19][17] + P[1][17] * SH_MAG[0] - P[2][17] * SH_MAG[1] + P[3][17] * SH_MAG[2] - P[16][17] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][17] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (2.0f * q0 * q2 - 2.0f * q1 * q3) * (P[19][18] + P[1][18] * SH_MAG[0] - P[2][18] * SH_MAG[1] + P[3][18] * SH_MAG[2] - P[16][18] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][18] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[19][0] + P[1][0] * SH_MAG[0] - P[2][0] * SH_MAG[1] + P[3][0] * SH_MAG[2] - P[16][0] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][0] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[17][19] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][19] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + SH_MAG[0] * (P[19][1] + P[1][1] * SH_MAG[0] - P[2][1] * SH_MAG[1] + P[3][1] * SH_MAG[2] - P[16][1] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][1] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - SH_MAG[1] * (P[19][2] + P[1][2] * SH_MAG[0] - P[2][2] * SH_MAG[1] + P[3][2] * SH_MAG[2] - P[16][2] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][2] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[2] * (P[19][3] + P[1][3] * SH_MAG[0] - P[2][3] * SH_MAG[1] + P[3][3] * SH_MAG[2] - P[16][3] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][3] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) * (P[19][16] + P[1][16] * SH_MAG[0] - P[2][16] * SH_MAG[1] + P[3][16] * SH_MAG[2] - P[16][16] * (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16] * (2.0f * q0 * q3 + 2.0f * q1 * q2) - P[18][16] * (2.0f * q0 * q2 - 2.0f * q1 * q3) + P[0][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[0][19] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + + //if (varInnovMag.X < R_MAG) + //{ + //UE_LOG(LogTemp, Display, TEXT("varInnovMag.X, R_MAG : %f, %f"), varInnovMag.X, R_MAG); + //CovarianceInit(); + //return; + //} + + varInnovMag.Y = (P[20][20] + R_MAG + P[0][20] * SH_MAG[2] + P[1][20] * SH_MAG[1] + P[2][20] * SH_MAG[0] - P[17][20] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f * q0 * q3 - 2.0f * q1 * q2) * (P[20][16] + P[0][16] * SH_MAG[2] + P[1][16] * SH_MAG[1] + P[2][16] * SH_MAG[0] - P[17][16] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][16] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (2.0f * q0 * q1 + 2.0f * q2 * q3) * (P[20][18] + P[0][18] * SH_MAG[2] + P[1][18] * SH_MAG[1] + P[2][18] * SH_MAG[0] - P[17][18] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][18] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[20][3] + P[0][3] * SH_MAG[2] + P[1][3] * SH_MAG[1] + P[2][3] * SH_MAG[0] - P[17][3] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][3] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - P[16][20] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][20] * (2.0f * q0 * q1 + 2.0f * q2 * q3) + SH_MAG[2] * (P[20][0] + P[0][0] * SH_MAG[2] + P[1][0] * SH_MAG[1] + P[2][0] * SH_MAG[0] - P[17][0] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][0] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[1] * (P[20][1] + P[0][1] * SH_MAG[2] + P[1][1] * SH_MAG[1] + P[2][1] * SH_MAG[0] - P[17][1] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][1] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[0] * (P[20][2] + P[0][2] * SH_MAG[2] + P[1][2] * SH_MAG[1] + P[2][2] * SH_MAG[0] - P[17][2] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][2] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) * (P[20][17] + P[0][17] * SH_MAG[2] + P[1][17] * SH_MAG[1] + P[2][17] * SH_MAG[0] - P[17][17] * (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17] * (2.0f * q0 * q3 - 2.0f * q1 * q2) + P[18][17] * (2.0f * q0 * q1 + 2.0f * q2 * q3) - P[3][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - P[3][20] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + + //if (varInnovMag.Y < R_MAG) + //{ + //UE_LOG(LogTemp, Display, TEXT("varInnovMag.Y, R_MAG : %f, %f"), varInnovMag.Y, R_MAG); + //CovarianceInit(); + //return; + //} + + varInnovMag.Z = (P[21][21] + R_MAG + P[0][21] * SH_MAG[1] - P[1][21] * SH_MAG[2] + P[3][21] * SH_MAG[0] + P[18][21] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f * q0 * q2 + 2.0f * q1 * q3) * (P[21][16] + P[0][16] * SH_MAG[1] - P[1][16] * SH_MAG[2] + P[3][16] * SH_MAG[0] + P[18][16] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][16] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][16] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - (2.0f * q0 * q1 - 2.0f * q2 * q3) * (P[21][17] + P[0][17] * SH_MAG[1] - P[1][17] * SH_MAG[2] + P[3][17] * SH_MAG[0] + P[18][17] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][17] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][17] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2) * (P[21][2] + P[0][2] * SH_MAG[1] - P[1][2] * SH_MAG[2] + P[3][2] * SH_MAG[0] + P[18][2] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][2] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][2] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[16][21] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][21] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + SH_MAG[1] * (P[21][0] + P[0][0] * SH_MAG[1] - P[1][0] * SH_MAG[2] + P[3][0] * SH_MAG[0] + P[18][0] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][0] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][0] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) - SH_MAG[2] * (P[21][1] + P[0][1] * SH_MAG[1] - P[1][1] * SH_MAG[2] + P[3][1] * SH_MAG[0] + P[18][1] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][1] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][1] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + SH_MAG[0] * (P[21][3] + P[0][3] * SH_MAG[1] - P[1][3] * SH_MAG[2] + P[3][3] * SH_MAG[0] + P[18][3] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][3] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][3] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) * (P[21][18] + P[0][18] * SH_MAG[1] - P[1][18] * SH_MAG[2] + P[3][18] * SH_MAG[0] + P[18][18] * (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18] * (2.0f * q0 * q2 + 2.0f * q1 * q3) - P[17][18] * (2.0f * q0 * q1 - 2.0f * q2 * q3) + P[2][18] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)) + P[2][21] * (SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2)); + + //if (varInnovMag.Z < R_MAG) + //{ + //UE_LOG(LogTemp, Display, TEXT("varInnovMag.Z, R_MAG : %f, %f"), varInnovMag.Z, R_MAG); + //CovarianceInit(); + //return; + //} + + FVector magTestRatio; + for (uint8_t i = 0; i <= 2; i++) + { + magTestRatio[i] = sq(innovMag[i]) / (sq(fmax(0.01f * _magInnovGate, 1.0f)) * varInnovMag[i]); + } + + bool magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f); + if (!magHealth) + { + UE_LOG(LogTemp, Warning, TEXT("MAGNETOMETER FUSION: BAD MAGNETOMETER HEALTH")); + //return; + } + + Vector22 H_MAG; + + for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) + { + if (obsIndex == 0) + { + for (uint8_t i = 0; i < NumStates; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = -SH_MAG[1]; + H_MAG[3] = SH_MAG[2]; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2.0f * q0 * q3 + 2.0f * q1 * q2; + H_MAG[18] = 2.0f * q1 * q3 - 2.0f * q0 * q2; + H_MAG[19] = 1.0f; + H_MAG[20] = 0.0f; + H_MAG[21] = 0.0f; + + const Vector5 SK_MX{ + 1.0f / varInnovMag[0], + SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q2 - 2.0f * q1 * q3, + 2.0f * q0 * q3 + 2.0f * q1 * q2 + }; + + Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][1] * SH_MAG[0] - P[0][2] * SH_MAG[1] + P[0][3] * SH_MAG[2] + P[0][0] * SK_MX[2] - P[0][16] * SK_MX[1] + P[0][17] * SK_MX[4] - P[0][18] * SK_MX[3]); + Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][1] * SH_MAG[0] - P[1][2] * SH_MAG[1] + P[1][3] * SH_MAG[2] + P[1][0] * SK_MX[2] - P[1][16] * SK_MX[1] + P[1][17] * SK_MX[4] - P[1][18] * SK_MX[3]); + Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][1] * SH_MAG[0] - P[2][2] * SH_MAG[1] + P[2][3] * SH_MAG[2] + P[2][0] * SK_MX[2] - P[2][16] * SK_MX[1] + P[2][17] * SK_MX[4] - P[2][18] * SK_MX[3]); + Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][1] * SH_MAG[0] - P[3][2] * SH_MAG[1] + P[3][3] * SH_MAG[2] + P[3][0] * SK_MX[2] - P[3][16] * SK_MX[1] + P[3][17] * SK_MX[4] - P[3][18] * SK_MX[3]); + Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][1] * SH_MAG[0] - P[4][2] * SH_MAG[1] + P[4][3] * SH_MAG[2] + P[4][0] * SK_MX[2] - P[4][16] * SK_MX[1] + P[4][17] * SK_MX[4] - P[4][18] * SK_MX[3]); + Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][1] * SH_MAG[0] - P[5][2] * SH_MAG[1] + P[5][3] * SH_MAG[2] + P[5][0] * SK_MX[2] - P[5][16] * SK_MX[1] + P[5][17] * SK_MX[4] - P[5][18] * SK_MX[3]); + Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][1] * SH_MAG[0] - P[6][2] * SH_MAG[1] + P[6][3] * SH_MAG[2] + P[6][0] * SK_MX[2] - P[6][16] * SK_MX[1] + P[6][17] * SK_MX[4] - P[6][18] * SK_MX[3]); + Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][1] * SH_MAG[0] - P[7][2] * SH_MAG[1] + P[7][3] * SH_MAG[2] + P[7][0] * SK_MX[2] - P[7][16] * SK_MX[1] + P[7][17] * SK_MX[4] - P[7][18] * SK_MX[3]); + Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][1] * SH_MAG[0] - P[8][2] * SH_MAG[1] + P[8][3] * SH_MAG[2] + P[8][0] * SK_MX[2] - P[8][16] * SK_MX[1] + P[8][17] * SK_MX[4] - P[8][18] * SK_MX[3]); + Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][1] * SH_MAG[0] - P[9][2] * SH_MAG[1] + P[9][3] * SH_MAG[2] + P[9][0] * SK_MX[2] - P[9][16] * SK_MX[1] + P[9][17] * SK_MX[4] - P[9][18] * SK_MX[3]); + Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][1] * SH_MAG[0] - P[10][2] * SH_MAG[1] + P[10][3] * SH_MAG[2] + P[10][0] * SK_MX[2] - P[10][16] * SK_MX[1] + P[10][17] * SK_MX[4] - P[10][18] * SK_MX[3]); + Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][1] * SH_MAG[0] - P[11][2] * SH_MAG[1] + P[11][3] * SH_MAG[2] + P[11][0] * SK_MX[2] - P[11][16] * SK_MX[1] + P[11][17] * SK_MX[4] - P[11][18] * SK_MX[3]); + Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][1] * SH_MAG[0] - P[12][2] * SH_MAG[1] + P[12][3] * SH_MAG[2] + P[12][0] * SK_MX[2] - P[12][16] * SK_MX[1] + P[12][17] * SK_MX[4] - P[12][18] * SK_MX[3]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MX[0] * (P[stateIndex][19] + P[stateIndex][1] * SH_MAG[0] - P[stateIndex][2] * SH_MAG[1] + P[stateIndex][3] * SH_MAG[2] + P[stateIndex][0] * SK_MX[2] - P[stateIndex][16] * SK_MX[1] + P[stateIndex][17] * SK_MX[4] - P[stateIndex][18] * SK_MX[3]); + } + + //for (uint8_t index = 16; index <= 21; index++) + //{ + //Kfusion[index] = 0.0; + //} + + Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][1] * SH_MAG[0] - P[16][2] * SH_MAG[1] + P[16][3] * SH_MAG[2] + P[16][0] * SK_MX[2] - P[16][16] * SK_MX[1] + P[16][17] * SK_MX[4] - P[16][18] * SK_MX[3]); + Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][1] * SH_MAG[0] - P[17][2] * SH_MAG[1] + P[17][3] * SH_MAG[2] + P[17][0] * SK_MX[2] - P[17][16] * SK_MX[1] + P[17][17] * SK_MX[4] - P[17][18] * SK_MX[3]); + Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][1] * SH_MAG[0] - P[18][2] * SH_MAG[1] + P[18][3] * SH_MAG[2] + P[18][0] * SK_MX[2] - P[18][16] * SK_MX[1] + P[18][17] * SK_MX[4] - P[18][18] * SK_MX[3]); + Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][1] * SH_MAG[0] - P[19][2] * SH_MAG[1] + P[19][3] * SH_MAG[2] + P[19][0] * SK_MX[2] - P[19][16] * SK_MX[1] + P[19][17] * SK_MX[4] - P[19][18] * SK_MX[3]); + Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][1] * SH_MAG[0] - P[20][2] * SH_MAG[1] + P[20][3] * SH_MAG[2] + P[20][0] * SK_MX[2] - P[20][16] * SK_MX[1] + P[20][17] * SK_MX[4] - P[20][18] * SK_MX[3]); + Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][1] * SH_MAG[0] - P[21][2] * SH_MAG[1] + P[21][3] * SH_MAG[2] + P[21][0] * SK_MX[2] - P[21][16] * SK_MX[1] + P[21][17] * SK_MX[4] - P[21][18] * SK_MX[3]); + } + else if (obsIndex == 1) + { + for (uint8_t i = 0; i < NumStates; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2.0f * magD * q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2.0f * q1 * q2 - 2.0f * q0 * q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2.0f * q0 * q1 + 2.0f * q2 * q3; + H_MAG[19] = 0.0f; + H_MAG[20] = 1.0f; + H_MAG[21] = 0.0f; + + const Vector5 SK_MY{ + 1.0f / varInnovMag[1], + SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q3 - 2.0f * q1 * q2, + 2.0f * q0 * q1 + 2.0f * q2 * q3 + }; + + Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][0] * SH_MAG[2] + P[0][1] * SH_MAG[1] + P[0][2] * SH_MAG[0] - P[0][3] * SK_MY[2] - P[0][17] * SK_MY[1] - P[0][16] * SK_MY[3] + P[0][18] * SK_MY[4]); + Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][0] * SH_MAG[2] + P[1][1] * SH_MAG[1] + P[1][2] * SH_MAG[0] - P[1][3] * SK_MY[2] - P[1][17] * SK_MY[1] - P[1][16] * SK_MY[3] + P[1][18] * SK_MY[4]); + Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][0] * SH_MAG[2] + P[2][1] * SH_MAG[1] + P[2][2] * SH_MAG[0] - P[2][3] * SK_MY[2] - P[2][17] * SK_MY[1] - P[2][16] * SK_MY[3] + P[2][18] * SK_MY[4]); + Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][0] * SH_MAG[2] + P[3][1] * SH_MAG[1] + P[3][2] * SH_MAG[0] - P[3][3] * SK_MY[2] - P[3][17] * SK_MY[1] - P[3][16] * SK_MY[3] + P[3][18] * SK_MY[4]); + Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][0] * SH_MAG[2] + P[4][1] * SH_MAG[1] + P[4][2] * SH_MAG[0] - P[4][3] * SK_MY[2] - P[4][17] * SK_MY[1] - P[4][16] * SK_MY[3] + P[4][18] * SK_MY[4]); + Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][0] * SH_MAG[2] + P[5][1] * SH_MAG[1] + P[5][2] * SH_MAG[0] - P[5][3] * SK_MY[2] - P[5][17] * SK_MY[1] - P[5][16] * SK_MY[3] + P[5][18] * SK_MY[4]); + Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][0] * SH_MAG[2] + P[6][1] * SH_MAG[1] + P[6][2] * SH_MAG[0] - P[6][3] * SK_MY[2] - P[6][17] * SK_MY[1] - P[6][16] * SK_MY[3] + P[6][18] * SK_MY[4]); + Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][0] * SH_MAG[2] + P[7][1] * SH_MAG[1] + P[7][2] * SH_MAG[0] - P[7][3] * SK_MY[2] - P[7][17] * SK_MY[1] - P[7][16] * SK_MY[3] + P[7][18] * SK_MY[4]); + Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][0] * SH_MAG[2] + P[8][1] * SH_MAG[1] + P[8][2] * SH_MAG[0] - P[8][3] * SK_MY[2] - P[8][17] * SK_MY[1] - P[8][16] * SK_MY[3] + P[8][18] * SK_MY[4]); + Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][0] * SH_MAG[2] + P[9][1] * SH_MAG[1] + P[9][2] * SH_MAG[0] - P[9][3] * SK_MY[2] - P[9][17] * SK_MY[1] - P[9][16] * SK_MY[3] + P[9][18] * SK_MY[4]); + Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][0] * SH_MAG[2] + P[10][1] * SH_MAG[1] + P[10][2] * SH_MAG[0] - P[10][3] * SK_MY[2] - P[10][17] * SK_MY[1] - P[10][16] * SK_MY[3] + P[10][18] * SK_MY[4]); + Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][0] * SH_MAG[2] + P[11][1] * SH_MAG[1] + P[11][2] * SH_MAG[0] - P[11][3] * SK_MY[2] - P[11][17] * SK_MY[1] - P[11][16] * SK_MY[3] + P[11][18] * SK_MY[4]); + Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][0] * SH_MAG[2] + P[12][1] * SH_MAG[1] + P[12][2] * SH_MAG[0] - P[12][3] * SK_MY[2] - P[12][17] * SK_MY[1] - P[12][16] * SK_MY[3] + P[12][18] * SK_MY[4]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MY[0] * (P[stateIndex][20] + P[stateIndex][0] * SH_MAG[2] + P[stateIndex][1] * SH_MAG[1] + P[stateIndex][2] * SH_MAG[0] - P[stateIndex][3] * SK_MY[2] - P[stateIndex][17] * SK_MY[1] - P[stateIndex][16] * SK_MY[3] + P[stateIndex][18] * SK_MY[4]); + } + + Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][0] * SH_MAG[2] + P[16][1] * SH_MAG[1] + P[16][2] * SH_MAG[0] - P[16][3] * SK_MY[2] - P[16][17] * SK_MY[1] - P[16][16] * SK_MY[3] + P[16][18] * SK_MY[4]); + Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][0] * SH_MAG[2] + P[17][1] * SH_MAG[1] + P[17][2] * SH_MAG[0] - P[17][3] * SK_MY[2] - P[17][17] * SK_MY[1] - P[17][16] * SK_MY[3] + P[17][18] * SK_MY[4]); + Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][0] * SH_MAG[2] + P[18][1] * SH_MAG[1] + P[18][2] * SH_MAG[0] - P[18][3] * SK_MY[2] - P[18][17] * SK_MY[1] - P[18][16] * SK_MY[3] + P[18][18] * SK_MY[4]); + Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][0] * SH_MAG[2] + P[19][1] * SH_MAG[1] + P[19][2] * SH_MAG[0] - P[19][3] * SK_MY[2] - P[19][17] * SK_MY[1] - P[19][16] * SK_MY[3] + P[19][18] * SK_MY[4]); + Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][0] * SH_MAG[2] + P[20][1] * SH_MAG[1] + P[20][2] * SH_MAG[0] - P[20][3] * SK_MY[2] - P[20][17] * SK_MY[1] - P[20][16] * SK_MY[3] + P[20][18] * SK_MY[4]); + Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][0] * SH_MAG[2] + P[21][1] * SH_MAG[1] + P[21][2] * SH_MAG[0] - P[21][3] * SK_MY[2] - P[21][17] * SK_MY[1] - P[21][16] * SK_MY[3] + P[21][18] * SK_MY[4]); + } + else if (obsIndex == 2) + { + for (uint8_t i = 0; i < NumStates; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = -SH_MAG[2]; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2.0f * q0 * q2 + 2.0f * q1 * q3; + H_MAG[17] = 2.0f * q2 * q3 - 2.0f * q0 * q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[19] = 0.0f; + H_MAG[20] = 0.0f; + H_MAG[21] = 1.0f; + + const Vector5 SK_MZ{ + 1.0f / varInnovMag[2], + SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6], + SH_MAG[7] + SH_MAG[8] - 2.0f * magD * q2, + 2.0f * q0 * q1 - 2.0f * q2 * q3, + 2.0f * q0 * q2 + 2.0f * q1 * q3 + }; + + Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][0] * SH_MAG[1] - P[0][1] * SH_MAG[2] + P[0][3] * SH_MAG[0] + P[0][2] * SK_MZ[2] + P[0][18] * SK_MZ[1] + P[0][16] * SK_MZ[4] - P[0][17] * SK_MZ[3]); + Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][0] * SH_MAG[1] - P[1][1] * SH_MAG[2] + P[1][3] * SH_MAG[0] + P[1][2] * SK_MZ[2] + P[1][18] * SK_MZ[1] + P[1][16] * SK_MZ[4] - P[1][17] * SK_MZ[3]); + Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][0] * SH_MAG[1] - P[2][1] * SH_MAG[2] + P[2][3] * SH_MAG[0] + P[2][2] * SK_MZ[2] + P[2][18] * SK_MZ[1] + P[2][16] * SK_MZ[4] - P[2][17] * SK_MZ[3]); + Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][0] * SH_MAG[1] - P[3][1] * SH_MAG[2] + P[3][3] * SH_MAG[0] + P[3][2] * SK_MZ[2] + P[3][18] * SK_MZ[1] + P[3][16] * SK_MZ[4] - P[3][17] * SK_MZ[3]); + Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][0] * SH_MAG[1] - P[4][1] * SH_MAG[2] + P[4][3] * SH_MAG[0] + P[4][2] * SK_MZ[2] + P[4][18] * SK_MZ[1] + P[4][16] * SK_MZ[4] - P[4][17] * SK_MZ[3]); + Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][0] * SH_MAG[1] - P[5][1] * SH_MAG[2] + P[5][3] * SH_MAG[0] + P[5][2] * SK_MZ[2] + P[5][18] * SK_MZ[1] + P[5][16] * SK_MZ[4] - P[5][17] * SK_MZ[3]); + Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][0] * SH_MAG[1] - P[6][1] * SH_MAG[2] + P[6][3] * SH_MAG[0] + P[6][2] * SK_MZ[2] + P[6][18] * SK_MZ[1] + P[6][16] * SK_MZ[4] - P[6][17] * SK_MZ[3]); + Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][0] * SH_MAG[1] - P[7][1] * SH_MAG[2] + P[7][3] * SH_MAG[0] + P[7][2] * SK_MZ[2] + P[7][18] * SK_MZ[1] + P[7][16] * SK_MZ[4] - P[7][17] * SK_MZ[3]); + Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][0] * SH_MAG[1] - P[8][1] * SH_MAG[2] + P[8][3] * SH_MAG[0] + P[8][2] * SK_MZ[2] + P[8][18] * SK_MZ[1] + P[8][16] * SK_MZ[4] - P[8][17] * SK_MZ[3]); + Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][0] * SH_MAG[1] - P[9][1] * SH_MAG[2] + P[9][3] * SH_MAG[0] + P[9][2] * SK_MZ[2] + P[9][18] * SK_MZ[1] + P[9][16] * SK_MZ[4] - P[9][17] * SK_MZ[3]); + Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][0] * SH_MAG[1] - P[10][1] * SH_MAG[2] + P[10][3] * SH_MAG[0] + P[10][2] * SK_MZ[2] + P[10][18] * SK_MZ[1] + P[10][16] * SK_MZ[4] - P[10][17] * SK_MZ[3]); + Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][0] * SH_MAG[1] - P[11][1] * SH_MAG[2] + P[11][3] * SH_MAG[0] + P[11][2] * SK_MZ[2] + P[11][18] * SK_MZ[1] + P[11][16] * SK_MZ[4] - P[11][17] * SK_MZ[3]); + Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][0] * SH_MAG[1] - P[12][1] * SH_MAG[2] + P[12][3] * SH_MAG[0] + P[12][2] * SK_MZ[2] + P[12][18] * SK_MZ[1] + P[12][16] * SK_MZ[4] - P[12][17] * SK_MZ[3]); + + for (uint8_t index = 0; index < 3; index++) + { + const uint8_t stateIndex = index + 13; + Kfusion[stateIndex] = SK_MZ[0] * (P[stateIndex][21] + P[stateIndex][0] * SH_MAG[1] - P[stateIndex][1] * SH_MAG[2] + P[stateIndex][3] * SH_MAG[0] + P[stateIndex][2] * SK_MZ[2] + P[stateIndex][18] * SK_MZ[1] + P[stateIndex][16] * SK_MZ[4] - P[stateIndex][17] * SK_MZ[3]); + } + + Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][0] * SH_MAG[1] - P[16][1] * SH_MAG[2] + P[16][3] * SH_MAG[0] + P[16][2] * SK_MZ[2] + P[16][18] * SK_MZ[1] + P[16][16] * SK_MZ[4] - P[16][17] * SK_MZ[3]); + Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][0] * SH_MAG[1] - P[17][1] * SH_MAG[2] + P[17][3] * SH_MAG[0] + P[17][2] * SK_MZ[2] + P[17][18] * SK_MZ[1] + P[17][16] * SK_MZ[4] - P[17][17] * SK_MZ[3]); + Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][0] * SH_MAG[1] - P[18][1] * SH_MAG[2] + P[18][3] * SH_MAG[0] + P[18][2] * SK_MZ[2] + P[18][18] * SK_MZ[1] + P[18][16] * SK_MZ[4] - P[18][17] * SK_MZ[3]); + Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][0] * SH_MAG[1] - P[19][1] * SH_MAG[2] + P[19][3] * SH_MAG[0] + P[19][2] * SK_MZ[2] + P[19][18] * SK_MZ[1] + P[19][16] * SK_MZ[4] - P[19][17] * SK_MZ[3]); + Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][0] * SH_MAG[1] - P[20][1] * SH_MAG[2] + P[20][3] * SH_MAG[0] + P[20][2] * SK_MZ[2] + P[20][18] * SK_MZ[1] + P[20][16] * SK_MZ[4] - P[20][17] * SK_MZ[3]); + Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][0] * SH_MAG[1] - P[21][1] * SH_MAG[2] + P[21][3] * SH_MAG[0] + P[21][2] * SK_MZ[2] + P[21][18] * SK_MZ[1] + P[21][16] * SK_MZ[4] - P[21][17] * SK_MZ[3]); + } + + for (int i = 0; i < NumStates; i++) + { + for (unsigned j = 0; j <= 3; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 4; j <= 15; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 16; j <= 21; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 22; j <= 23; j++) { + KH[i][j] = 0.0f; + } + } + for (int j = 0; j < NumStates; j++) + { + for (int i = 0; i < NumStates; i++) + { + float res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][16] * P[16][j]; + res += KH[i][17] * P[17][j]; + res += KH[i][18] * P[18][j]; + res += KH[i][19] * P[19][j]; + res += KH[i][20] * P[20][j]; + res += KH[i][21] * P[21][j]; + KHP[i][j] = res; + } + } + bool healthyFusion = true; + for (uint8_t i = 0; i < NumStates; i++) + { + if (KHP[i][i] > P[i][i]) + { + if (abs(KHP[i][i]) < 1e-6) + { + KHP[i][i] = 0.0; + } + else + { + //UE_LOG(LogTemp, Display, TEXT("Magnetometer fusion KHP[%d][%d], P[%d][%d] : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]); + healthyFusion = false; + } + } + } + if (healthyFusion) + { + for (uint8_t i = 0; i < NumStates; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + ForceSymmetry(); + ConstrainVariances(); + FillStateArray(); + for (uint8_t j = 0; j < NumStates; j++) + { + StateArray[j] = StateArray[j] - Kfusion[j] * innovMag[obsIndex]; + } + FillStateStruct(); + State.quat.Normalize(); + quat2arr(); + } + else + { + CovarianceInit(); + return; + } + } +} + +void EKFilter::VelPosFusion() +{ + if (!readBaro() || !readGPS()) + { + return; + } + + FVector gps_vel(recentGPS.VelocityNorth, recentGPS.VelocityEast, recentGPS.VelocityUP); + FVector innovations = State.velocity - gps_vel; + //const float obs_data_chk = sq(constrain_value(_gpsHorizVelNoise, 0.05, 5.0)) + sq(gpsNEVelVarAccScale * accNavMag);; + const float obs_data_chk = sq(constrain_value(_gpsHorizVelNoise, 0.05, 5.0)); + float posErr = gpsPosVarAccScale * accNavMag; + FVector variances; + variances.X = P[4][4] + obs_data_chk; + variances.Y = P[5][5] + obs_data_chk; + variances.Z = P[6][6] + obs_data_chk; + + Vector6 velPosObs; + velPosObs[0] = recentGPS.VelocityNorth; + velPosObs[1] = recentGPS.VelocityEast; + velPosObs[2] = recentGPS.VelocityUP; + Location gpsloc; + gpsloc.SetPositionGeodetic(recentGPS.LonDeg, recentGPS.LatDeg, 0); + FVector posxy = Origin.GetDistanceNED(gpsloc); + velPosObs[3] = posxy.X; + velPosObs[4] = posxy.Y; + + float posDownObsNoise; + float hgtMea = recentGPS.Alt; + switch (HeightSource) + { + case Sensor::BARO: + posDownObsNoise = sq(constrain_value(_baroAltNoise, 0.1f, 100.0f)); + hgtMea = -recentBaro.altitude; + break; + case Sensor::GPS: + posDownObsNoise = sq(_gpsVertPosNoise); + hgtMea = recentGPS.Alt; + } + velPosObs[5] = hgtMea; + + bool fuseData[6]{}; + uint8_t stateIndex; + uint8_t obsIndex; + + Vector6 R_OBS; + Vector6 R_OBS_DATA_CHECKS; + float SK; + + //R_OBS[0] = sq(_gpsHorizVelNoise) + sq(gpsNEVelVarAccScale * accNavMag); + R_OBS[0] = sq(_gpsHorizVelNoise); + //R_OBS[2] = sq(_gpsVertVelNoise) + sq(gpsDVelVarAccScale * accNavMag);; + R_OBS[2] = sq(_gpsVertVelNoise); + R_OBS[1] = R_OBS[0]; + //R_OBS[3] = sq(constrain_value(gpsPosAccuracy, _gpsHorizPosNoise, 100.0f)); + R_OBS[3] = sq(_gpsHorizPosNoise); + R_OBS[4] = R_OBS[3]; + switch (HeightSource) + { + case Sensor::BARO: + R_OBS[5] = sq(_baroAltNoise); + break; + case Sensor::GPS: + R_OBS[5] = sq(_gpsVertPosNoise); + break; + } + + R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk; + + for (uint8_t i = 3; i <= 5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; + + innovVelPos[3] = State.position.X - velPosObs[3]; + innovVelPos[4] = State.position.Y - velPosObs[4]; + varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4]; + float maxPosInnov2 = sq(fmax(0.01 * _gpsPosInnovGate, 1.0)) * (varInnovVelPos[3] + varInnovVelPos[4]); + float posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; + bool posCheckPassed = false; + bool fusePosData = true; + if (posTestRatio < 1.0f) posCheckPassed = true; + + //if (posCheckPassed) + //{ + //bool posVarianceIsTooLarge = (_gpsGlitchRadiusMax > 0) && ((P[8][8] + P[7][7]) > sq(_gpsGlitchRadiusMax)); + //bool posVarianceIsTooLarge = P[8][8] > (2 * sq(_gpsHorizPosNoise)) || P[7][7] > (2 * sq(_gpsHorizPosNoise)); + //if (posVarianceIsTooLarge) + //{ + //UE_LOG(LogTemp, Warning, TEXT("POSITION VARIANCE TOO LARGE! RESETING")); + //zeroRows(P, 7, 8); + //zeroCols(P, 7, 8); + //P[7][7] = sq(0.5f * _gpsGlitchRadiusMax); + //P[7][7] = sq(_gpsHorizPosNoise); + //P[8][8] = P[7][7]; + //State.position.X = posxy.X; + //State.position.Y = posxy.Y; + + //fusePosData = false; + //} + //} + //} + //else + //{ + //fusePosData = false; + //UE_LOG(LogTemp, Warning, TEXT("WON'T FUSE POSITION")); + //} + /* + bool fuseVelData = true; + float innovVelSumSq = 0; // sum of squares of velocity innovations + float varVelSum = 0; // sum of velocity innovation variances + uint8_t imax = 2; + for (uint8_t i = 0; i <= imax; i++) + { + stateIndex = i + 4; + const float vel_innov = State.velocity[i] - velPosObs[i]; + innovVelSumSq += sq(vel_innov); + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; + varVelSum += varInnovVelPos[i]; + } + float velTestRatio = innovVelSumSq / (varVelSum * sq(fmax(0.01 * _gpsVelInnovGate, 1.0))); + bool velCheckPassed = false; + if (velTestRatio < 1.0) + { + velCheckPassed = true; + } + + if (velCheckPassed) + { + //bool velVarianceIsTooLarge = P[4][4] > (2 * sq(_gpsHorizVelNoise)) || P[5][5] > (2 * sq(_gpsHorizVelNoise)); + //if (velVarianceIsTooLarge) + //{ + UE_LOG(LogTemp, Warning, TEXT("VELOCITY RESETING %f"), velTestRatio); + zeroRows(P, 4, 5); + zeroCols(P, 4, 5); + State.velocity.X = recentGPS.VelocityNorth; + State.velocity.Y = recentGPS.VelocityEast; + P[5][5] = P[4][4] = sq(_gpsHorizVelNoise); + //fuseVelData = false; + //} + } + + bool fuseHgtData; + innovVelPos[5] = State.position.Z - velPosObs[5]; + varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; + float hgtTestRatio = sq(innovVelPos[5]) / (sq(fmax(0.01 * _hgtInnovGate, 1.0)) * varInnovVelPos[5]); + const float maxTestRatio = 1.0f; + bool hgtCheckPassed = false; + if (hgtTestRatio < maxTestRatio) + { + hgtCheckPassed = true; + } + //else if(HeightSource == Sensor::GPS) + //{ + //varInnovVelPos[5] *= hgtTestRatio; + //hgtCheckPassed = true; + //} + if (hgtCheckPassed) + { + fuseHgtData = false; + //bool HgtVarianceIsTooLarge = P[9][9] > (2 * posDownObsNoise) || P[6][6] > (2 * sq(_gpsVertVelNoise)); + //if (HgtVarianceIsTooLarge) + //{ + UE_LOG(LogTemp, Warning, TEXT("ALTITUDE RESETING")); + zeroRows(P, 9, 9); + zeroCols(P, 9, 9); + P[9][9] = posDownObsNoise; + zeroRows(P, 6, 6); + zeroCols(P, 6, 6); + P[6][6] = sq(_gpsVertVelNoise); + State.position.Z = hgtMea; + State.velocity.Z = recentGPS.VelocityUP; + //} + } + */ + //UE_LOG(LogTemp, Warning, TEXT("Velocity before fusion: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.velocity before fusion: %f, %f, %f!"), State.velocity.X, State.velocity.Y, State.velocity.Z) + + for (obsIndex = 0; obsIndex <= 5; obsIndex++) + { + stateIndex = 4 + obsIndex; + if (obsIndex <= 2) + { + innovVelPos[obsIndex] = State.velocity[obsIndex] - velPosObs[obsIndex]; + //R_OBS[obsIndex] *= sq(gpsNoiseScaler); + //UE_LOG(LogTemp, Warning, TEXT("Velocity innovation: %f!"), innovVelPos[obsIndex]) + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = State.position[obsIndex - 3] - velPosObs[obsIndex]; + //UE_LOG(LogTemp, Warning, TEXT("Position XY innovation: %f!"), innovVelPos[obsIndex]) + //R_OBS[obsIndex] *= sq(gpsNoiseScaler); + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = State.position[obsIndex - 3] - velPosObs[obsIndex]; + //UE_LOG(LogTemp, Warning, TEXT("Position Z innovation: %f!"), innovVelPos[obsIndex]) + } + + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0f / varInnovVelPos[obsIndex]; + for (uint8_t i = 0; i <= 9; i++) + { + Kfusion[i] = P[i][stateIndex] * SK; + } + + float M_SQRT1_2 = 0.70710678118654752440; + + for (uint8_t i = 10; i <= 12; i++) + { + bool poorObservability = false; + const uint8_t axisIndex = i - 10; + if (axisIndex == 0) + { + poorObservability = fabsf(prevTnb.A().Z) > M_SQRT1_2; + } + else if (axisIndex == 1) + { + poorObservability = fabsf(prevTnb.B().Z) > M_SQRT1_2; + } + else + { + poorObservability = fabsf(prevTnb.C().Z) > M_SQRT1_2; + } + if (poorObservability) + { + Kfusion[i] = 0.0; + } + else + { + Kfusion[i] = P[i][stateIndex] * SK; + } + } + + for (uint8_t i = 13; i < NumStates; i++) + { + Kfusion[i] = P[i][stateIndex] * SK; + } + + for (uint8_t i = 0; i < NumStates; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + + bool healthyFusion = true; + for (uint8_t i = 0; i < NumStates; i++) + { + if (KHP[i][i] > P[i][i]) + { + if (KHP[i][i] < 1e-6) + { + KHP[i][i] = 0.0; + } + else + { + //UE_LOG(LogTemp, Display, TEXT("Velocity Position KHP[%d][%d], P[%d][%d] : %f, %f"), i, i, i, i, KHP[i][i], P[i][i]); + healthyFusion = false; + } + } + } + + if (healthyFusion) + { + for (uint8_t i = 0; i < NumStates; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + if (j == i && KHP[i][j] > P[i][j]) + { + P[i][j] = 0.0; + } + else + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + FillStateArray(); + for (uint8_t i = 0; i < NumStates; i++) + { + StateArray[i] = StateArray[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + FillStateStruct(); + State.quat.Normalize(); + quat2arr(); + } + //else + //{ + //UE_LOG(LogTemp, Warning, TEXT("NOT HEALTHY VELOCITY FUSION")); + //} + } + //UE_LOG(LogTemp, Warning, TEXT("Velocity after fusion: %f, %f, %f!"), statesArray[4], statesArray[5], statesArray[6]) + //UE_LOG(LogTemp, Warning, TEXT("State.elocity: %f, %f, %f!"),State.velocity.X, State.velocity.Y, State.velocity.Z) +} + +void EKFilter::Update() +{ + if (readIMUData()) + { + UpdateState(); + } + CovariancePrediction(nullptr); + MagFusion(); + VelPosFusion(); + + //readMagData(); + //readGPS(); + //readBaro(); +} + +void EKFilter::CovarianceInit() +{ + float sq_var = 0.1f * 0.1f; + FVector rot_vec_var(sq_var, sq_var, sq_var); + + CovariancePrediction(&rot_vec_var); + + float dtEkfAvg = (recentAccel.dt + recentGyro.dt) * 0.5f; + + // velocities + P[4][4] = sq(_gpsHorizVelNoise); + P[5][5] = P[4][4]; + P[6][6] = sq(_gpsVertVelNoise); + // positions + P[7][7] = sq(_gpsHorizPosNoise); + P[8][8] = P[7][7]; + + switch (HeightSource) + { + case Sensor::GPS: + P[9][9] = sq(_gpsVertPosNoise); + break; + case Sensor::BARO: + P[9][9] = sq(_baroAltNoise); + break; + default: + UE_LOG(LogTemp, Error, TEXT("Wrong height data source")); + break; + } + // gyro delta angle biases + P[10][10] = sq(radians(2.5f * dtEkfAvg)); + //P[10][10] = 0.001; + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + // delta velocity biases + P[13][13] = sq(0.2f * _accBiasLim * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + // earth magnetic field + P[16][16] = sq(_magNoise); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + // body magnetic field + P[19][19] = sq(_magNoise); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; +} + +void EKFilter::CovariancePrediction(FVector* rotVarVectPtr) +{ + float daxVar; // X axis delta angle noise variance rad^2 + float dayVar; // Y axis delta angle noise variance rad^2 + float dazVar; // Z axis delta angle noise variance rad^2 + float dvxVar; // X axis delta velocity variance noise (m/s)^2 + float dvyVar; // Y axis delta velocity variance noise (m/s)^2 + float dvzVar; // Z axis delta velocity variance noise (m/s)^2 + float dvx; // X axis delta velocity (m/s) + float dvy; // Y axis delta velocity (m/s) + float dvz; // Z axis delta velocity (m/s) + float dax; // X axis delta angle (rad) + float day; // Y axis delta angle (rad) + float daz; // Z axis delta angle (rad) + float q0; // attitude quaternion + float q1; // attitude quaternion + float q2; // attitude quaternion + float q3; // attitude quaternion + float dax_b; // X axis delta angle measurement bias (rad) + float day_b; // Y axis delta angle measurement bias (rad) + float daz_b; // Z axis delta angle measurement bias (rad) + float dvx_b; // X axis delta velocity measurement bias (rad) + float dvy_b; // Y axis delta velocity measurement bias (rad) + float dvz_b; // Z axis delta velocity measurement bias (rad) + + float cov_dt = 0.5 * (recentGyro.dt + recentAccel.dt); + + float processNoiseVariance[12] = {}; + + for (uint8_t i = 0; i <= 2; i++) processNoiseVariance[i] = sq(sq(cov_dt) * _gyroBiasProcessNoise); + for (uint8_t i = 3; i <= 5; i++) processNoiseVariance[i] = sq(sq(cov_dt) * _accelBiasProcessNoise); + for (uint8_t i = 6; i <= 8; i++) processNoiseVariance[i] = sq(cov_dt * _magEarthProcessNoise); + for (uint8_t i = 9; i <= 11; i++) processNoiseVariance[i] = sq(cov_dt * _magBodyProcessNoise); + + bool quatCovResetOnly = false; + if (rotVarVectPtr != nullptr) + { + const FVector& rotVarVec = *rotVarVectPtr; + Matrix33 R_ef(rotVarVec.X, 0.0f, 0.0f, 0.0f, rotVarVec.Y, 0.0f, 0.0f, 0.0f, rotVarVec.Z); + Matrix33 Tnb = State.quat.Inverse().GetMatrix(); + Matrix33 R_bf = Tnb * R_ef * Tnb.Transposed(); + daxVar = R_bf.A().X; + dayVar = R_bf.B().Y; + dazVar = R_bf.C().Z; + quatCovResetOnly = true; + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); + } + + daxVar = dayVar = dazVar = sq(cov_dt * _gyrNoise); + dvxVar = dvyVar = dvzVar = sq(cov_dt * _accNoise); + + //daxVar = dayVar = dazVar = sq(_gyrNoise); + //dvxVar = dvyVar = dvzVar = sq(_accNoise); + + dvx = recentAccel.data.X; + dvy = recentAccel.data.Y; + dvz = recentAccel.data.Z; + dax = recentGyro.data.X; + day = recentGyro.data.Y; + daz = recentGyro.data.Z; + q0 = State.quat(0); + q1 = State.quat(1); + q2 = State.quat(2); + q3 = State.quat(3); + dax_b = State.gyro_bias.X; + day_b = State.gyro_bias.Y; + daz_b = State.gyro_bias.Z; + dvx_b = State.accel_bias.X; + dvy_b = State.accel_bias.Y; + dvz_b = State.accel_bias.Z; + + const float PS0 = sq(q1); + const float PS1 = 0.25F * daxVar; + const float PS2 = sq(q2); + const float PS3 = 0.25F * dayVar; + const float PS4 = sq(q3); + const float PS5 = 0.25F * dazVar; + const float PS6 = 0.5F * q1; + const float PS7 = 0.5F * q2; + const float PS8 = PS7 * P[10][11]; + const float PS9 = 0.5F * q3; + const float PS10 = PS9 * P[10][12]; + const float PS11 = 0.5F * dax - 0.5F * dax_b; + const float PS12 = 0.5F * day - 0.5F * day_b; + const float PS13 = 0.5F * daz - 0.5F * daz_b; + const float PS14 = PS10 - PS11 * P[1][10] - PS12 * P[2][10] - PS13 * P[3][10] + PS6 * P[10][10] + PS8 + P[0][10]; + const float PS15 = PS6 * P[10][11]; + const float PS16 = PS9 * P[11][12]; + const float PS17 = -PS11 * P[1][11] - PS12 * P[2][11] - PS13 * P[3][11] + PS15 + PS16 + PS7 * P[11][11] + P[0][11]; + const float PS18 = PS6 * P[10][12]; + const float PS19 = PS7 * P[11][12]; + const float PS20 = -PS11 * P[1][12] - PS12 * P[2][12] - PS13 * P[3][12] + PS18 + PS19 + PS9 * P[12][12] + P[0][12]; + const float PS21 = PS12 * P[1][2]; + const float PS22 = -PS13 * P[1][3]; + const float PS23 = -PS11 * P[1][1] - PS21 + PS22 + PS6 * P[1][10] + PS7 * P[1][11] + PS9 * P[1][12] + P[0][1]; + const float PS24 = -PS11 * P[1][2]; + const float PS25 = PS13 * P[2][3]; + const float PS26 = -PS12 * P[2][2] + PS24 - PS25 + PS6 * P[2][10] + PS7 * P[2][11] + PS9 * P[2][12] + P[0][2]; + const float PS27 = PS11 * P[1][3]; + const float PS28 = -PS12 * P[2][3]; + const float PS29 = -PS13 * P[3][3] - PS27 + PS28 + PS6 * P[3][10] + PS7 * P[3][11] + PS9 * P[3][12] + P[0][3]; + const float PS30 = PS11 * P[0][1]; + const float PS31 = PS12 * P[0][2]; + const float PS32 = PS13 * P[0][3]; + const float PS33 = -PS30 - PS31 - PS32 + PS6 * P[0][10] + PS7 * P[0][11] + PS9 * P[0][12] + P[0][0]; + const float PS34 = 0.5F * q0; + const float PS35 = q2 * q3; + const float PS36 = q0 * q1; + const float PS37 = q1 * q3; + const float PS38 = q0 * q2; + const float PS39 = q1 * q2; + const float PS40 = q0 * q3; + const float PS41 = 2 * PS2; + const float PS42 = 2 * PS4 - 1; + const float PS43 = PS41 + PS42; + const float PS44 = -PS11 * P[1][13] - PS12 * P[2][13] - PS13 * P[3][13] + PS6 * P[10][13] + PS7 * P[11][13] + PS9 * P[12][13] + P[0][13]; + const float PS45 = PS37 + PS38; + const float PS46 = -PS11 * P[1][15] - PS12 * P[2][15] - PS13 * P[3][15] + PS6 * P[10][15] + PS7 * P[11][15] + PS9 * P[12][15] + P[0][15]; + const float PS47 = 2 * PS46; + const float PS48 = dvy - dvy_b; + const float PS49 = PS48 * q0; + const float PS50 = dvz - dvz_b; + const float PS51 = PS50 * q1; + const float PS52 = dvx - dvx_b; + const float PS53 = PS52 * q3; + const float PS54 = PS49 - PS51 + 2 * PS53; + const float PS55 = 2 * PS29; + const float PS56 = -PS39 + PS40; + const float PS57 = -PS11 * P[1][14] - PS12 * P[2][14] - PS13 * P[3][14] + PS6 * P[10][14] + PS7 * P[11][14] + PS9 * P[12][14] + P[0][14]; + const float PS58 = 2 * PS57; + const float PS59 = PS48 * q2; + const float PS60 = PS50 * q3; + const float PS61 = PS59 + PS60; + const float PS62 = 2 * PS23; + const float PS63 = PS50 * q2; + const float PS64 = PS48 * q3; + const float PS65 = -PS64; + const float PS66 = PS63 + PS65; + const float PS67 = 2 * PS33; + const float PS68 = PS50 * q0; + const float PS69 = PS48 * q1; + const float PS70 = PS52 * q2; + const float PS71 = PS68 + PS69 - 2 * PS70; + const float PS72 = 2 * PS26; + const float PS73 = -PS11 * P[1][4] - PS12 * P[2][4] - PS13 * P[3][4] + PS6 * P[4][10] + PS7 * P[4][11] + PS9 * P[4][12] + P[0][4]; + const float PS74 = 2 * PS0; + const float PS75 = PS42 + PS74; + const float PS76 = PS39 + PS40; + const float PS77 = 2 * PS44; + const float PS78 = PS51 - PS53; + const float PS79 = -PS70; + const float PS80 = PS68 + 2 * PS69 + PS79; + const float PS81 = -PS35 + PS36; + const float PS82 = PS52 * q1; + const float PS83 = PS60 + PS82; + const float PS84 = PS52 * q0; + const float PS85 = PS63 - 2 * PS64 + PS84; + const float PS86 = -PS11 * P[1][5] - PS12 * P[2][5] - PS13 * P[3][5] + PS6 * P[5][10] + PS7 * P[5][11] + PS9 * P[5][12] + P[0][5]; + const float PS87 = PS41 + PS74 - 1; + const float PS88 = PS35 + PS36; + const float PS89 = 2 * PS63 + PS65 + PS84; + const float PS90 = -PS37 + PS38; + const float PS91 = PS59 + PS82; + const float PS92 = PS69 + PS79; + const float PS93 = PS49 - 2 * PS51 + PS53; + const float PS94 = -PS11 * P[1][6] - PS12 * P[2][6] - PS13 * P[3][6] + PS6 * P[6][10] + PS7 * P[6][11] + PS9 * P[6][12] + P[0][6]; + const float PS95 = sq(q0); + const float PS96 = -PS34 * P[10][11]; + const float PS97 = PS11 * P[0][11] - PS12 * P[3][11] + PS13 * P[2][11] - PS19 + PS9 * P[11][11] + PS96 + P[1][11]; + const float PS98 = PS13 * P[0][2]; + const float PS99 = PS12 * P[0][3]; + const float PS100 = PS11 * P[0][0] - PS34 * P[0][10] - PS7 * P[0][12] + PS9 * P[0][11] + PS98 - PS99 + P[0][1]; + const float PS101 = PS11 * P[0][2]; + const float PS102 = PS101 + PS13 * P[2][2] + PS28 - PS34 * P[2][10] - PS7 * P[2][12] + PS9 * P[2][11] + P[1][2]; + const float PS103 = PS9 * P[10][11]; + const float PS104 = PS7 * P[10][12]; + const float PS105 = PS103 - PS104 + PS11 * P[0][10] - PS12 * P[3][10] + PS13 * P[2][10] - PS34 * P[10][10] + P[1][10]; + const float PS106 = -PS34 * P[10][12]; + const float PS107 = PS106 + PS11 * P[0][12] - PS12 * P[3][12] + PS13 * P[2][12] + PS16 - PS7 * P[12][12] + P[1][12]; + const float PS108 = PS11 * P[0][3]; + const float PS109 = PS108 - PS12 * P[3][3] + PS25 - PS34 * P[3][10] - PS7 * P[3][12] + PS9 * P[3][11] + P[1][3]; + const float PS110 = PS13 * P[1][2]; + const float PS111 = PS12 * P[1][3]; + const float PS112 = PS110 - PS111 + PS30 - PS34 * P[1][10] - PS7 * P[1][12] + PS9 * P[1][11] + P[1][1]; + const float PS113 = PS11 * P[0][13] - PS12 * P[3][13] + PS13 * P[2][13] - PS34 * P[10][13] - PS7 * P[12][13] + PS9 * P[11][13] + P[1][13]; + const float PS114 = PS11 * P[0][15] - PS12 * P[3][15] + PS13 * P[2][15] - PS34 * P[10][15] - PS7 * P[12][15] + PS9 * P[11][15] + P[1][15]; + const float PS115 = 2 * PS114; + const float PS116 = 2 * PS109; + const float PS117 = PS11 * P[0][14] - PS12 * P[3][14] + PS13 * P[2][14] - PS34 * P[10][14] - PS7 * P[12][14] + PS9 * P[11][14] + P[1][14]; + const float PS118 = 2 * PS117; + const float PS119 = 2 * PS112; + const float PS120 = 2 * PS100; + const float PS121 = 2 * PS102; + const float PS122 = PS11 * P[0][4] - PS12 * P[3][4] + PS13 * P[2][4] - PS34 * P[4][10] - PS7 * P[4][12] + PS9 * P[4][11] + P[1][4]; + const float PS123 = 2 * PS113; + const float PS124 = PS11 * P[0][5] - PS12 * P[3][5] + PS13 * P[2][5] - PS34 * P[5][10] - PS7 * P[5][12] + PS9 * P[5][11] + P[1][5]; + const float PS125 = PS11 * P[0][6] - PS12 * P[3][6] + PS13 * P[2][6] - PS34 * P[6][10] - PS7 * P[6][12] + PS9 * P[6][11] + P[1][6]; + const float PS126 = -PS34 * P[11][12]; + const float PS127 = -PS10 + PS11 * P[3][12] + PS12 * P[0][12] + PS126 - PS13 * P[1][12] + PS6 * P[12][12] + P[2][12]; + const float PS128 = PS11 * P[3][3] + PS22 - PS34 * P[3][11] + PS6 * P[3][12] - PS9 * P[3][10] + PS99 + P[2][3]; + const float PS129 = PS13 * P[0][1]; + const float PS130 = PS108 + PS12 * P[0][0] - PS129 - PS34 * P[0][11] + PS6 * P[0][12] - PS9 * P[0][10] + P[0][2]; + const float PS131 = PS6 * P[11][12]; + const float PS132 = -PS103 + PS11 * P[3][11] + PS12 * P[0][11] - PS13 * P[1][11] + PS131 - PS34 * P[11][11] + P[2][11]; + const float PS133 = PS11 * P[3][10] + PS12 * P[0][10] - PS13 * P[1][10] + PS18 - PS9 * P[10][10] + PS96 + P[2][10]; + const float PS134 = PS12 * P[0][1]; + const float PS135 = -PS13 * P[1][1] + PS134 + PS27 - PS34 * P[1][11] + PS6 * P[1][12] - PS9 * P[1][10] + P[1][2]; + const float PS136 = PS11 * P[2][3]; + const float PS137 = -PS110 + PS136 + PS31 - PS34 * P[2][11] + PS6 * P[2][12] - PS9 * P[2][10] + P[2][2]; + const float PS138 = PS11 * P[3][13] + PS12 * P[0][13] - PS13 * P[1][13] - PS34 * P[11][13] + PS6 * P[12][13] - PS9 * P[10][13] + P[2][13]; + const float PS139 = PS11 * P[3][15] + PS12 * P[0][15] - PS13 * P[1][15] - PS34 * P[11][15] + PS6 * P[12][15] - PS9 * P[10][15] + P[2][15]; + const float PS140 = 2 * PS139; + const float PS141 = 2 * PS128; + const float PS142 = PS11 * P[3][14] + PS12 * P[0][14] - PS13 * P[1][14] - PS34 * P[11][14] + PS6 * P[12][14] - PS9 * P[10][14] + P[2][14]; + const float PS143 = 2 * PS142; + const float PS144 = 2 * PS135; + const float PS145 = 2 * PS130; + const float PS146 = 2 * PS137; + const float PS147 = PS11 * P[3][4] + PS12 * P[0][4] - PS13 * P[1][4] - PS34 * P[4][11] + PS6 * P[4][12] - PS9 * P[4][10] + P[2][4]; + const float PS148 = 2 * PS138; + const float PS149 = PS11 * P[3][5] + PS12 * P[0][5] - PS13 * P[1][5] - PS34 * P[5][11] + PS6 * P[5][12] - PS9 * P[5][10] + P[2][5]; + const float PS150 = PS11 * P[3][6] + PS12 * P[0][6] - PS13 * P[1][6] - PS34 * P[6][11] + PS6 * P[6][12] - PS9 * P[6][10] + P[2][6]; + const float PS151 = PS106 - PS11 * P[2][10] + PS12 * P[1][10] + PS13 * P[0][10] - PS15 + PS7 * P[10][10] + P[3][10]; + const float PS152 = PS12 * P[1][1] + PS129 + PS24 - PS34 * P[1][12] - PS6 * P[1][11] + PS7 * P[1][10] + P[1][3]; + const float PS153 = -PS101 + PS13 * P[0][0] + PS134 - PS34 * P[0][12] - PS6 * P[0][11] + PS7 * P[0][10] + P[0][3]; + const float PS154 = PS104 - PS11 * P[2][12] + PS12 * P[1][12] + PS13 * P[0][12] - PS131 - PS34 * P[12][12] + P[3][12]; + const float PS155 = -PS11 * P[2][11] + PS12 * P[1][11] + PS126 + PS13 * P[0][11] - PS6 * P[11][11] + PS8 + P[3][11]; + const float PS156 = -PS11 * P[2][2] + PS21 - PS34 * P[2][12] - PS6 * P[2][11] + PS7 * P[2][10] + PS98 + P[2][3]; + const float PS157 = PS111 - PS136 + PS32 - PS34 * P[3][12] - PS6 * P[3][11] + PS7 * P[3][10] + P[3][3]; + const float PS158 = -PS11 * P[2][13] + PS12 * P[1][13] + PS13 * P[0][13] - PS34 * P[12][13] - PS6 * P[11][13] + PS7 * P[10][13] + P[3][13]; + const float PS159 = -PS11 * P[2][15] + PS12 * P[1][15] + PS13 * P[0][15] - PS34 * P[12][15] - PS6 * P[11][15] + PS7 * P[10][15] + P[3][15]; + const float PS160 = 2 * PS159; + const float PS161 = 2 * PS157; + const float PS162 = -PS11 * P[2][14] + PS12 * P[1][14] + PS13 * P[0][14] - PS34 * P[12][14] - PS6 * P[11][14] + PS7 * P[10][14] + P[3][14]; + const float PS163 = 2 * PS162; + const float PS164 = 2 * PS152; + const float PS165 = 2 * PS153; + const float PS166 = 2 * PS156; + const float PS167 = -PS11 * P[2][4] + PS12 * P[1][4] + PS13 * P[0][4] - PS34 * P[4][12] - PS6 * P[4][11] + PS7 * P[4][10] + P[3][4]; + const float PS168 = 2 * PS158; + const float PS169 = -PS11 * P[2][5] + PS12 * P[1][5] + PS13 * P[0][5] - PS34 * P[5][12] - PS6 * P[5][11] + PS7 * P[5][10] + P[3][5]; + const float PS170 = -PS11 * P[2][6] + PS12 * P[1][6] + PS13 * P[0][6] - PS34 * P[6][12] - PS6 * P[6][11] + PS7 * P[6][10] + P[3][6]; + const float PS171 = 2 * PS45; + const float PS172 = 2 * PS56; + const float PS173 = 2 * PS61; + const float PS174 = 2 * PS66; + const float PS175 = 2 * PS71; + const float PS176 = 2 * PS54; + const float PS177 = -PS171 * P[13][15] + PS172 * P[13][14] + PS173 * P[1][13] + PS174 * P[0][13] + PS175 * P[2][13] - PS176 * P[3][13] + PS43 * P[13][13] + P[4][13]; + const float PS178 = -PS171 * P[15][15] + PS172 * P[14][15] + PS173 * P[1][15] + PS174 * P[0][15] + PS175 * P[2][15] - PS176 * P[3][15] + PS43 * P[13][15] + P[4][15]; + const float PS179 = -PS171 * P[3][15] + PS172 * P[3][14] + PS173 * P[1][3] + PS174 * P[0][3] + PS175 * P[2][3] - PS176 * P[3][3] + PS43 * P[3][13] + P[3][4]; + const float PS180 = -PS171 * P[14][15] + PS172 * P[14][14] + PS173 * P[1][14] + PS174 * P[0][14] + PS175 * P[2][14] - PS176 * P[3][14] + PS43 * P[13][14] + P[4][14]; + const float PS181 = -PS171 * P[1][15] + PS172 * P[1][14] + PS173 * P[1][1] + PS174 * P[0][1] + PS175 * P[1][2] - PS176 * P[1][3] + PS43 * P[1][13] + P[1][4]; + const float PS182 = -PS171 * P[0][15] + PS172 * P[0][14] + PS173 * P[0][1] + PS174 * P[0][0] + PS175 * P[0][2] - PS176 * P[0][3] + PS43 * P[0][13] + P[0][4]; + const float PS183 = -PS171 * P[2][15] + PS172 * P[2][14] + PS173 * P[1][2] + PS174 * P[0][2] + PS175 * P[2][2] - PS176 * P[2][3] + PS43 * P[2][13] + P[2][4]; + const float PS184 = 4 * dvyVar; + const float PS185 = 4 * dvzVar; + const float PS186 = -PS171 * P[4][15] + PS172 * P[4][14] + PS173 * P[1][4] + PS174 * P[0][4] + PS175 * P[2][4] - PS176 * P[3][4] + PS43 * P[4][13] + P[4][4]; + const float PS187 = 2 * PS177; + const float PS188 = 2 * PS182; + const float PS189 = 2 * PS181; + const float PS190 = 2 * PS81; + const float PS191 = 2 * PS183; + const float PS192 = 2 * PS179; + const float PS193 = 2 * PS76; + const float PS194 = PS43 * dvxVar; + const float PS195 = PS75 * dvyVar; + const float PS196 = -PS171 * P[5][15] + PS172 * P[5][14] + PS173 * P[1][5] + PS174 * P[0][5] + PS175 * P[2][5] - PS176 * P[3][5] + PS43 * P[5][13] + P[4][5]; + const float PS197 = 2 * PS88; + const float PS198 = PS87 * dvzVar; + const float PS199 = 2 * PS90; + const float PS200 = -PS171 * P[6][15] + PS172 * P[6][14] + PS173 * P[1][6] + PS174 * P[0][6] + PS175 * P[2][6] - PS176 * P[3][6] + PS43 * P[6][13] + P[4][6]; + const float PS201 = 2 * PS83; + const float PS202 = 2 * PS78; + const float PS203 = 2 * PS85; + const float PS204 = 2 * PS80; + const float PS205 = PS190 * P[14][15] - PS193 * P[13][14] + PS201 * P[2][14] - PS202 * P[0][14] + PS203 * P[3][14] - PS204 * P[1][14] + PS75 * P[14][14] + P[5][14]; + const float PS206 = PS190 * P[13][15] - PS193 * P[13][13] + PS201 * P[2][13] - PS202 * P[0][13] + PS203 * P[3][13] - PS204 * P[1][13] + PS75 * P[13][14] + P[5][13]; + const float PS207 = PS190 * P[0][15] - PS193 * P[0][13] + PS201 * P[0][2] - PS202 * P[0][0] + PS203 * P[0][3] - PS204 * P[0][1] + PS75 * P[0][14] + P[0][5]; + const float PS208 = PS190 * P[1][15] - PS193 * P[1][13] + PS201 * P[1][2] - PS202 * P[0][1] + PS203 * P[1][3] - PS204 * P[1][1] + PS75 * P[1][14] + P[1][5]; + const float PS209 = PS190 * P[15][15] - PS193 * P[13][15] + PS201 * P[2][15] - PS202 * P[0][15] + PS203 * P[3][15] - PS204 * P[1][15] + PS75 * P[14][15] + P[5][15]; + const float PS210 = PS190 * P[2][15] - PS193 * P[2][13] + PS201 * P[2][2] - PS202 * P[0][2] + PS203 * P[2][3] - PS204 * P[1][2] + PS75 * P[2][14] + P[2][5]; + const float PS211 = PS190 * P[3][15] - PS193 * P[3][13] + PS201 * P[2][3] - PS202 * P[0][3] + PS203 * P[3][3] - PS204 * P[1][3] + PS75 * P[3][14] + P[3][5]; + const float PS212 = 4 * dvxVar; + const float PS213 = PS190 * P[5][15] - PS193 * P[5][13] + PS201 * P[2][5] - PS202 * P[0][5] + PS203 * P[3][5] - PS204 * P[1][5] + PS75 * P[5][14] + P[5][5]; + const float PS214 = 2 * PS89; + const float PS215 = 2 * PS91; + const float PS216 = 2 * PS92; + const float PS217 = 2 * PS93; + const float PS218 = PS190 * P[6][15] - PS193 * P[6][13] + PS201 * P[2][6] - PS202 * P[0][6] + PS203 * P[3][6] - PS204 * P[1][6] + PS75 * P[6][14] + P[5][6]; + const float PS219 = -PS197 * P[14][15] + PS199 * P[13][15] - PS214 * P[2][15] + PS215 * P[3][15] + PS216 * P[0][15] + PS217 * P[1][15] + PS87 * P[15][15] + P[6][15]; + const float PS220 = -PS197 * P[14][14] + PS199 * P[13][14] - PS214 * P[2][14] + PS215 * P[3][14] + PS216 * P[0][14] + PS217 * P[1][14] + PS87 * P[14][15] + P[6][14]; + const float PS221 = -PS197 * P[13][14] + PS199 * P[13][13] - PS214 * P[2][13] + PS215 * P[3][13] + PS216 * P[0][13] + PS217 * P[1][13] + PS87 * P[13][15] + P[6][13]; + const float PS222 = -PS197 * P[6][14] + PS199 * P[6][13] - PS214 * P[2][6] + PS215 * P[3][6] + PS216 * P[0][6] + PS217 * P[1][6] + PS87 * P[6][15] + P[6][6]; + + nextP[0][0] = PS0 * PS1 - PS11 * PS23 - PS12 * PS26 - PS13 * PS29 + PS14 * PS6 + PS17 * PS7 + PS2 * PS3 + PS20 * PS9 + PS33 + PS4 * PS5; + nextP[0][1] = -PS1 * PS36 + PS11 * PS33 - PS12 * PS29 + PS13 * PS26 - PS14 * PS34 + PS17 * PS9 - PS20 * PS7 + PS23 + PS3 * PS35 - PS35 * PS5; + nextP[1][1] = PS1 * PS95 + PS100 * PS11 + PS102 * PS13 - PS105 * PS34 - PS107 * PS7 - PS109 * PS12 + PS112 + PS2 * PS5 + PS3 * PS4 + PS9 * PS97; + nextP[0][2] = -PS1 * PS37 + PS11 * PS29 + PS12 * PS33 - PS13 * PS23 - PS14 * PS9 - PS17 * PS34 + PS20 * PS6 + PS26 - PS3 * PS38 + PS37 * PS5; + nextP[1][2] = PS1 * PS40 + PS100 * PS12 + PS102 - PS105 * PS9 + PS107 * PS6 + PS109 * PS11 - PS112 * PS13 - PS3 * PS40 - PS34 * PS97 - PS39 * PS5; + nextP[2][2] = PS0 * PS5 + PS1 * PS4 + PS11 * PS128 + PS12 * PS130 + PS127 * PS6 - PS13 * PS135 - PS132 * PS34 - PS133 * PS9 + PS137 + PS3 * PS95; + nextP[0][3] = PS1 * PS39 - PS11 * PS26 + PS12 * PS23 + PS13 * PS33 + PS14 * PS7 - PS17 * PS6 - PS20 * PS34 + PS29 - PS3 * PS39 - PS40 * PS5; + nextP[1][3] = -PS1 * PS38 + PS100 * PS13 - PS102 * PS11 + PS105 * PS7 - PS107 * PS34 + PS109 + PS112 * PS12 - PS3 * PS37 + PS38 * PS5 - PS6 * PS97; + nextP[2][3] = -PS1 * PS35 - PS11 * PS137 + PS12 * PS135 - PS127 * PS34 + PS128 + PS13 * PS130 - PS132 * PS6 + PS133 * PS7 + PS3 * PS36 - PS36 * PS5; + nextP[3][3] = PS0 * PS3 + PS1 * PS2 - PS11 * PS156 + PS12 * PS152 + PS13 * PS153 + PS151 * PS7 - PS154 * PS34 - PS155 * PS6 + PS157 + PS5 * PS95; + + if (quatCovResetOnly) + { + // covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP + // to lower and upper half in P + for (uint8_t row = 0; row <= 3; row++) + { + // copy diagonals + P[row][row] = constrain_value(nextP[row][row], 0.0f, 1.0f); + // copy off diagonals + for (uint8_t column = 0; column < row; column++) + { + P[row][column] = P[column][row] = nextP[column][row]; + } + } + calcTiltErrorVariance(); + return; + } + + nextP[0][4] = PS43 * PS44 - PS45 * PS47 - PS54 * PS55 + PS56 * PS58 + PS61 * PS62 + PS66 * PS67 + PS71 * PS72 + PS73; + nextP[1][4] = PS113 * PS43 - PS115 * PS45 - PS116 * PS54 + PS118 * PS56 + PS119 * PS61 + PS120 * PS66 + PS121 * PS71 + PS122; + nextP[2][4] = PS138 * PS43 - PS140 * PS45 - PS141 * PS54 + PS143 * PS56 + PS144 * PS61 + PS145 * PS66 + PS146 * PS71 + PS147; + nextP[3][4] = PS158 * PS43 - PS160 * PS45 - PS161 * PS54 + PS163 * PS56 + PS164 * PS61 + PS165 * PS66 + PS166 * PS71 + PS167; + nextP[4][4] = -PS171 * PS178 + PS172 * PS180 + PS173 * PS181 + PS174 * PS182 + PS175 * PS183 - PS176 * PS179 + PS177 * PS43 + PS184 * sq(PS56) + PS185 * sq(PS45) + PS186 + sq(PS43) * dvxVar; + nextP[0][5] = PS47 * PS81 + PS55 * PS85 + PS57 * PS75 - PS62 * PS80 - PS67 * PS78 + PS72 * PS83 - PS76 * PS77 + PS86; + nextP[1][5] = PS115 * PS81 + PS116 * PS85 + PS117 * PS75 - PS119 * PS80 - PS120 * PS78 + PS121 * PS83 - PS123 * PS76 + PS124; + nextP[2][5] = PS140 * PS81 + PS141 * PS85 + PS142 * PS75 - PS144 * PS80 - PS145 * PS78 + PS146 * PS83 - PS148 * PS76 + PS149; + nextP[3][5] = PS160 * PS81 + PS161 * PS85 + PS162 * PS75 - PS164 * PS80 - PS165 * PS78 + PS166 * PS83 - PS168 * PS76 + PS169; + nextP[4][5] = PS172 * PS195 + PS178 * PS190 + PS180 * PS75 - PS185 * PS45 * PS81 - PS187 * PS76 - PS188 * PS78 - PS189 * PS80 + PS191 * PS83 + PS192 * PS85 - PS193 * PS194 + PS196; + nextP[5][5] = PS185 * sq(PS81) + PS190 * PS209 - PS193 * PS206 + PS201 * PS210 - PS202 * PS207 + PS203 * PS211 - PS204 * PS208 + PS205 * PS75 + PS212 * sq(PS76) + PS213 + sq(PS75) * dvyVar; + nextP[0][6] = PS46 * PS87 + PS55 * PS91 - PS58 * PS88 + PS62 * PS93 + PS67 * PS92 - PS72 * PS89 + PS77 * PS90 + PS94; + nextP[1][6] = PS114 * PS87 + PS116 * PS91 - PS118 * PS88 + PS119 * PS93 + PS120 * PS92 - PS121 * PS89 + PS123 * PS90 + PS125; + nextP[2][6] = PS139 * PS87 + PS141 * PS91 - PS143 * PS88 + PS144 * PS93 + PS145 * PS92 - PS146 * PS89 + PS148 * PS90 + PS150; + nextP[3][6] = PS159 * PS87 + PS161 * PS91 - PS163 * PS88 + PS164 * PS93 + PS165 * PS92 - PS166 * PS89 + PS168 * PS90 + PS170; + nextP[4][6] = -PS171 * PS198 + PS178 * PS87 - PS180 * PS197 - PS184 * PS56 * PS88 + PS187 * PS90 + PS188 * PS92 + PS189 * PS93 - PS191 * PS89 + PS192 * PS91 + PS194 * PS199 + PS200; + nextP[5][6] = PS190 * PS198 - PS195 * PS197 - PS197 * PS205 + PS199 * PS206 + PS207 * PS216 + PS208 * PS217 + PS209 * PS87 - PS210 * PS214 + PS211 * PS215 - PS212 * PS76 * PS90 + PS218; + nextP[6][6] = PS184 * sq(PS88) - PS197 * PS220 + PS199 * PS221 + PS212 * sq(PS90) - PS214 * (-PS197 * P[2][14] + PS199 * P[2][13] - PS214 * P[2][2] + PS215 * P[2][3] + PS216 * P[0][2] + PS217 * P[1][2] + PS87 * P[2][15] + P[2][6]) + PS215 * (-PS197 * P[3][14] + PS199 * P[3][13] - PS214 * P[2][3] + PS215 * P[3][3] + PS216 * P[0][3] + PS217 * P[1][3] + PS87 * P[3][15] + P[3][6]) + PS216 * (-PS197 * P[0][14] + PS199 * P[0][13] - PS214 * P[0][2] + PS215 * P[0][3] + PS216 * P[0][0] + PS217 * P[0][1] + PS87 * P[0][15] + P[0][6]) + PS217 * (-PS197 * P[1][14] + PS199 * P[1][13] - PS214 * P[1][2] + PS215 * P[1][3] + PS216 * P[0][1] + PS217 * P[1][1] + PS87 * P[1][15] + P[1][6]) + PS219 * PS87 + PS222 + sq(PS87) * dvzVar; + nextP[0][7] = -PS11 * P[1][7] - PS12 * P[2][7] - PS13 * P[3][7] + PS6 * P[7][10] + PS7 * P[7][11] + PS73 * cov_dt + PS9 * P[7][12] + P[0][7]; + nextP[1][7] = PS11 * P[0][7] - PS12 * P[3][7] + PS122 * cov_dt + PS13 * P[2][7] - PS34 * P[7][10] - PS7 * P[7][12] + PS9 * P[7][11] + P[1][7]; + nextP[2][7] = PS11 * P[3][7] + PS12 * P[0][7] - PS13 * P[1][7] + PS147 * cov_dt - PS34 * P[7][11] + PS6 * P[7][12] - PS9 * P[7][10] + P[2][7]; + nextP[3][7] = -PS11 * P[2][7] + PS12 * P[1][7] + PS13 * P[0][7] + PS167 * cov_dt - PS34 * P[7][12] - PS6 * P[7][11] + PS7 * P[7][10] + P[3][7]; + nextP[4][7] = -PS171 * P[7][15] + PS172 * P[7][14] + PS173 * P[1][7] + PS174 * P[0][7] + PS175 * P[2][7] - PS176 * P[3][7] + PS186 * cov_dt + PS43 * P[7][13] + P[4][7]; + nextP[5][7] = PS190 * P[7][15] - PS193 * P[7][13] + PS201 * P[2][7] - PS202 * P[0][7] + PS203 * P[3][7] - PS204 * P[1][7] + PS75 * P[7][14] + P[5][7] + cov_dt * (PS190 * P[4][15] - PS193 * P[4][13] + PS201 * P[2][4] - PS202 * P[0][4] + PS203 * P[3][4] - PS204 * P[1][4] + PS75 * P[4][14] + P[4][5]); + nextP[6][7] = -PS197 * P[7][14] + PS199 * P[7][13] - PS214 * P[2][7] + PS215 * P[3][7] + PS216 * P[0][7] + PS217 * P[1][7] + PS87 * P[7][15] + P[6][7] + cov_dt * (-PS197 * P[4][14] + PS199 * P[4][13] - PS214 * P[2][4] + PS215 * P[3][4] + PS216 * P[0][4] + PS217 * P[1][4] + PS87 * P[4][15] + P[4][6]); + nextP[7][7] = P[4][7] * cov_dt + P[7][7] + cov_dt * (P[4][4] * cov_dt + P[4][7]); + nextP[0][8] = -PS11 * P[1][8] - PS12 * P[2][8] - PS13 * P[3][8] + PS6 * P[8][10] + PS7 * P[8][11] + PS86 * cov_dt + PS9 * P[8][12] + P[0][8]; + nextP[1][8] = PS11 * P[0][8] - PS12 * P[3][8] + PS124 * cov_dt + PS13 * P[2][8] - PS34 * P[8][10] - PS7 * P[8][12] + PS9 * P[8][11] + P[1][8]; + nextP[2][8] = PS11 * P[3][8] + PS12 * P[0][8] - PS13 * P[1][8] + PS149 * cov_dt - PS34 * P[8][11] + PS6 * P[8][12] - PS9 * P[8][10] + P[2][8]; + nextP[3][8] = -PS11 * P[2][8] + PS12 * P[1][8] + PS13 * P[0][8] + PS169 * cov_dt - PS34 * P[8][12] - PS6 * P[8][11] + PS7 * P[8][10] + P[3][8]; + nextP[4][8] = -PS171 * P[8][15] + PS172 * P[8][14] + PS173 * P[1][8] + PS174 * P[0][8] + PS175 * P[2][8] - PS176 * P[3][8] + PS196 * cov_dt + PS43 * P[8][13] + P[4][8]; + nextP[5][8] = PS190 * P[8][15] - PS193 * P[8][13] + PS201 * P[2][8] - PS202 * P[0][8] + PS203 * P[3][8] - PS204 * P[1][8] + PS213 * cov_dt + PS75 * P[8][14] + P[5][8]; + nextP[6][8] = -PS197 * P[8][14] + PS199 * P[8][13] - PS214 * P[2][8] + PS215 * P[3][8] + PS216 * P[0][8] + PS217 * P[1][8] + PS87 * P[8][15] + P[6][8] + cov_dt * (-PS197 * P[5][14] + PS199 * P[5][13] - PS214 * P[2][5] + PS215 * P[3][5] + PS216 * P[0][5] + PS217 * P[1][5] + PS87 * P[5][15] + P[5][6]); + nextP[7][8] = P[4][8] * cov_dt + P[7][8] + cov_dt * (P[4][5] * cov_dt + P[5][7]); + nextP[8][8] = P[5][8] * cov_dt + P[8][8] + cov_dt * (P[5][5] * cov_dt + P[5][8]); + nextP[0][9] = -PS11 * P[1][9] - PS12 * P[2][9] - PS13 * P[3][9] + PS6 * P[9][10] + PS7 * P[9][11] + PS9 * P[9][12] + PS94 * cov_dt + P[0][9]; + nextP[1][9] = PS11 * P[0][9] - PS12 * P[3][9] + PS125 * cov_dt + PS13 * P[2][9] - PS34 * P[9][10] - PS7 * P[9][12] + PS9 * P[9][11] + P[1][9]; + nextP[2][9] = PS11 * P[3][9] + PS12 * P[0][9] - PS13 * P[1][9] + PS150 * cov_dt - PS34 * P[9][11] + PS6 * P[9][12] - PS9 * P[9][10] + P[2][9]; + nextP[3][9] = -PS11 * P[2][9] + PS12 * P[1][9] + PS13 * P[0][9] + PS170 * cov_dt - PS34 * P[9][12] - PS6 * P[9][11] + PS7 * P[9][10] + P[3][9]; + nextP[4][9] = -PS171 * P[9][15] + PS172 * P[9][14] + PS173 * P[1][9] + PS174 * P[0][9] + PS175 * P[2][9] - PS176 * P[3][9] + PS200 * cov_dt + PS43 * P[9][13] + P[4][9]; + nextP[5][9] = PS190 * P[9][15] - PS193 * P[9][13] + PS201 * P[2][9] - PS202 * P[0][9] + PS203 * P[3][9] - PS204 * P[1][9] + PS218 * cov_dt + PS75 * P[9][14] + P[5][9]; + nextP[6][9] = -PS197 * P[9][14] + PS199 * P[9][13] - PS214 * P[2][9] + PS215 * P[3][9] + PS216 * P[0][9] + PS217 * P[1][9] + PS222 * cov_dt + PS87 * P[9][15] + P[6][9]; + nextP[7][9] = P[4][9] * cov_dt + P[7][9] + cov_dt * (P[4][6] * cov_dt + P[6][7]); + nextP[8][9] = P[5][9] * cov_dt + P[8][9] + cov_dt * (P[5][6] * cov_dt + P[6][8]); + nextP[9][9] = P[6][9] * cov_dt + P[9][9] + cov_dt * (P[6][6] * cov_dt + P[6][9]); + nextP[0][10] = PS14; + nextP[1][10] = PS105; + nextP[2][10] = PS133; + nextP[3][10] = PS151; + nextP[4][10] = -PS171 * P[10][15] + PS172 * P[10][14] + PS173 * P[1][10] + PS174 * P[0][10] + PS175 * P[2][10] - PS176 * P[3][10] + PS43 * P[10][13] + P[4][10]; + nextP[5][10] = PS190 * P[10][15] - PS193 * P[10][13] + PS201 * P[2][10] - PS202 * P[0][10] + PS203 * P[3][10] - PS204 * P[1][10] + PS75 * P[10][14] + P[5][10]; + nextP[6][10] = -PS197 * P[10][14] + PS199 * P[10][13] - PS214 * P[2][10] + PS215 * P[3][10] + PS216 * P[0][10] + PS217 * P[1][10] + PS87 * P[10][15] + P[6][10]; + nextP[7][10] = P[4][10] * cov_dt + P[7][10]; + nextP[8][10] = P[5][10] * cov_dt + P[8][10]; + nextP[9][10] = P[6][10] * cov_dt + P[9][10]; + nextP[10][10] = P[10][10]; + nextP[0][11] = PS17; + nextP[1][11] = PS97; + nextP[2][11] = PS132; + nextP[3][11] = PS155; + nextP[4][11] = -PS171 * P[11][15] + PS172 * P[11][14] + PS173 * P[1][11] + PS174 * P[0][11] + PS175 * P[2][11] - PS176 * P[3][11] + PS43 * P[11][13] + P[4][11]; + nextP[5][11] = PS190 * P[11][15] - PS193 * P[11][13] + PS201 * P[2][11] - PS202 * P[0][11] + PS203 * P[3][11] - PS204 * P[1][11] + PS75 * P[11][14] + P[5][11]; + nextP[6][11] = -PS197 * P[11][14] + PS199 * P[11][13] - PS214 * P[2][11] + PS215 * P[3][11] + PS216 * P[0][11] + PS217 * P[1][11] + PS87 * P[11][15] + P[6][11]; + nextP[7][11] = P[4][11] * cov_dt + P[7][11]; + nextP[8][11] = P[5][11] * cov_dt + P[8][11]; + nextP[9][11] = P[6][11] * cov_dt + P[9][11]; + nextP[10][11] = P[10][11]; + nextP[11][11] = P[11][11]; + nextP[0][12] = PS20; + nextP[1][12] = PS107; + nextP[2][12] = PS127; + nextP[3][12] = PS154; + nextP[4][12] = -PS171 * P[12][15] + PS172 * P[12][14] + PS173 * P[1][12] + PS174 * P[0][12] + PS175 * P[2][12] - PS176 * P[3][12] + PS43 * P[12][13] + P[4][12]; + nextP[5][12] = PS190 * P[12][15] - PS193 * P[12][13] + PS201 * P[2][12] - PS202 * P[0][12] + PS203 * P[3][12] - PS204 * P[1][12] + PS75 * P[12][14] + P[5][12]; + nextP[6][12] = -PS197 * P[12][14] + PS199 * P[12][13] - PS214 * P[2][12] + PS215 * P[3][12] + PS216 * P[0][12] + PS217 * P[1][12] + PS87 * P[12][15] + P[6][12]; + nextP[7][12] = P[4][12] * cov_dt + P[7][12]; + nextP[8][12] = P[5][12] * cov_dt + P[8][12]; + nextP[9][12] = P[6][12] * cov_dt + P[9][12]; + nextP[10][12] = P[10][12]; + nextP[11][12] = P[11][12]; + nextP[12][12] = P[12][12]; + nextP[0][13] = PS44; + nextP[1][13] = PS113; + nextP[2][13] = PS138; + nextP[3][13] = PS158; + nextP[4][13] = PS177; + nextP[5][13] = PS206; + nextP[6][13] = PS221; + nextP[7][13] = P[4][13] * cov_dt + P[7][13]; + nextP[8][13] = P[5][13] * cov_dt + P[8][13]; + nextP[9][13] = P[6][13] * cov_dt + P[9][13]; + nextP[10][13] = P[10][13]; + nextP[11][13] = P[11][13]; + nextP[12][13] = P[12][13]; + nextP[13][13] = P[13][13]; + nextP[0][14] = PS57; + nextP[1][14] = PS117; + nextP[2][14] = PS142; + nextP[3][14] = PS162; + nextP[4][14] = PS180; + nextP[5][14] = PS205; + nextP[6][14] = PS220; + nextP[7][14] = P[4][14] * cov_dt + P[7][14]; + nextP[8][14] = P[5][14] * cov_dt + P[8][14]; + nextP[9][14] = P[6][14] * cov_dt + P[9][14]; + nextP[10][14] = P[10][14]; + nextP[11][14] = P[11][14]; + nextP[12][14] = P[12][14]; + nextP[13][14] = P[13][14]; + nextP[14][14] = P[14][14]; + nextP[0][15] = PS46; + nextP[1][15] = PS114; + nextP[2][15] = PS139; + nextP[3][15] = PS159; + nextP[4][15] = PS178; + nextP[5][15] = PS209; + nextP[6][15] = PS219; + nextP[7][15] = P[4][15] * cov_dt + P[7][15]; + nextP[8][15] = P[5][15] * cov_dt + P[8][15]; + nextP[9][15] = P[6][15] * cov_dt + P[9][15]; + nextP[10][15] = P[10][15]; + nextP[11][15] = P[11][15]; + nextP[12][15] = P[12][15]; + nextP[13][15] = P[13][15]; + nextP[14][15] = P[14][15]; + nextP[15][15] = P[15][15]; + nextP[0][16] = -PS11 * P[1][16] - PS12 * P[2][16] - PS13 * P[3][16] + PS6 * P[10][16] + PS7 * P[11][16] + PS9 * P[12][16] + P[0][16]; + nextP[1][16] = PS11 * P[0][16] - PS12 * P[3][16] + PS13 * P[2][16] - PS34 * P[10][16] - PS7 * P[12][16] + PS9 * P[11][16] + P[1][16]; + nextP[2][16] = PS11 * P[3][16] + PS12 * P[0][16] - PS13 * P[1][16] - PS34 * P[11][16] + PS6 * P[12][16] - PS9 * P[10][16] + P[2][16]; + nextP[3][16] = -PS11 * P[2][16] + PS12 * P[1][16] + PS13 * P[0][16] - PS34 * P[12][16] - PS6 * P[11][16] + PS7 * P[10][16] + P[3][16]; + nextP[4][16] = -PS171 * P[15][16] + PS172 * P[14][16] + PS173 * P[1][16] + PS174 * P[0][16] + PS175 * P[2][16] - PS176 * P[3][16] + PS43 * P[13][16] + P[4][16]; + nextP[5][16] = PS190 * P[15][16] - PS193 * P[13][16] + PS201 * P[2][16] - PS202 * P[0][16] + PS203 * P[3][16] - PS204 * P[1][16] + PS75 * P[14][16] + P[5][16]; + nextP[6][16] = -PS197 * P[14][16] + PS199 * P[13][16] - PS214 * P[2][16] + PS215 * P[3][16] + PS216 * P[0][16] + PS217 * P[1][16] + PS87 * P[15][16] + P[6][16]; + nextP[7][16] = P[4][16] * cov_dt + P[7][16]; + nextP[8][16] = P[5][16] * cov_dt + P[8][16]; + nextP[9][16] = P[6][16] * cov_dt + P[9][16]; + nextP[10][16] = P[10][16]; + nextP[11][16] = P[11][16]; + nextP[12][16] = P[12][16]; + nextP[13][16] = P[13][16]; + nextP[14][16] = P[14][16]; + nextP[15][16] = P[15][16]; + nextP[16][16] = P[16][16]; + nextP[0][17] = -PS11 * P[1][17] - PS12 * P[2][17] - PS13 * P[3][17] + PS6 * P[10][17] + PS7 * P[11][17] + PS9 * P[12][17] + P[0][17]; + nextP[1][17] = PS11 * P[0][17] - PS12 * P[3][17] + PS13 * P[2][17] - PS34 * P[10][17] - PS7 * P[12][17] + PS9 * P[11][17] + P[1][17]; + nextP[2][17] = PS11 * P[3][17] + PS12 * P[0][17] - PS13 * P[1][17] - PS34 * P[11][17] + PS6 * P[12][17] - PS9 * P[10][17] + P[2][17]; + nextP[3][17] = -PS11 * P[2][17] + PS12 * P[1][17] + PS13 * P[0][17] - PS34 * P[12][17] - PS6 * P[11][17] + PS7 * P[10][17] + P[3][17]; + nextP[4][17] = -PS171 * P[15][17] + PS172 * P[14][17] + PS173 * P[1][17] + PS174 * P[0][17] + PS175 * P[2][17] - PS176 * P[3][17] + PS43 * P[13][17] + P[4][17]; + nextP[5][17] = PS190 * P[15][17] - PS193 * P[13][17] + PS201 * P[2][17] - PS202 * P[0][17] + PS203 * P[3][17] - PS204 * P[1][17] + PS75 * P[14][17] + P[5][17]; + nextP[6][17] = -PS197 * P[14][17] + PS199 * P[13][17] - PS214 * P[2][17] + PS215 * P[3][17] + PS216 * P[0][17] + PS217 * P[1][17] + PS87 * P[15][17] + P[6][17]; + nextP[7][17] = P[4][17] * cov_dt + P[7][17]; + nextP[8][17] = P[5][17] * cov_dt + P[8][17]; + nextP[9][17] = P[6][17] * cov_dt + P[9][17]; + nextP[10][17] = P[10][17]; + nextP[11][17] = P[11][17]; + nextP[12][17] = P[12][17]; + nextP[13][17] = P[13][17]; + nextP[14][17] = P[14][17]; + nextP[15][17] = P[15][17]; + nextP[16][17] = P[16][17]; + nextP[17][17] = P[17][17]; + nextP[0][18] = -PS11 * P[1][18] - PS12 * P[2][18] - PS13 * P[3][18] + PS6 * P[10][18] + PS7 * P[11][18] + PS9 * P[12][18] + P[0][18]; + nextP[1][18] = PS11 * P[0][18] - PS12 * P[3][18] + PS13 * P[2][18] - PS34 * P[10][18] - PS7 * P[12][18] + PS9 * P[11][18] + P[1][18]; + nextP[2][18] = PS11 * P[3][18] + PS12 * P[0][18] - PS13 * P[1][18] - PS34 * P[11][18] + PS6 * P[12][18] - PS9 * P[10][18] + P[2][18]; + nextP[3][18] = -PS11 * P[2][18] + PS12 * P[1][18] + PS13 * P[0][18] - PS34 * P[12][18] - PS6 * P[11][18] + PS7 * P[10][18] + P[3][18]; + nextP[4][18] = -PS171 * P[15][18] + PS172 * P[14][18] + PS173 * P[1][18] + PS174 * P[0][18] + PS175 * P[2][18] - PS176 * P[3][18] + PS43 * P[13][18] + P[4][18]; + nextP[5][18] = PS190 * P[15][18] - PS193 * P[13][18] + PS201 * P[2][18] - PS202 * P[0][18] + PS203 * P[3][18] - PS204 * P[1][18] + PS75 * P[14][18] + P[5][18]; + nextP[6][18] = -PS197 * P[14][18] + PS199 * P[13][18] - PS214 * P[2][18] + PS215 * P[3][18] + PS216 * P[0][18] + PS217 * P[1][18] + PS87 * P[15][18] + P[6][18]; + nextP[7][18] = P[4][18] * cov_dt + P[7][18]; + nextP[8][18] = P[5][18] * cov_dt + P[8][18]; + nextP[9][18] = P[6][18] * cov_dt + P[9][18]; + nextP[10][18] = P[10][18]; + nextP[11][18] = P[11][18]; + nextP[12][18] = P[12][18]; + nextP[13][18] = P[13][18]; + nextP[14][18] = P[14][18]; + nextP[15][18] = P[15][18]; + nextP[16][18] = P[16][18]; + nextP[17][18] = P[17][18]; + nextP[18][18] = P[18][18]; + nextP[0][19] = -PS11 * P[1][19] - PS12 * P[2][19] - PS13 * P[3][19] + PS6 * P[10][19] + PS7 * P[11][19] + PS9 * P[12][19] + P[0][19]; + nextP[1][19] = PS11 * P[0][19] - PS12 * P[3][19] + PS13 * P[2][19] - PS34 * P[10][19] - PS7 * P[12][19] + PS9 * P[11][19] + P[1][19]; + nextP[2][19] = PS11 * P[3][19] + PS12 * P[0][19] - PS13 * P[1][19] - PS34 * P[11][19] + PS6 * P[12][19] - PS9 * P[10][19] + P[2][19]; + nextP[3][19] = -PS11 * P[2][19] + PS12 * P[1][19] + PS13 * P[0][19] - PS34 * P[12][19] - PS6 * P[11][19] + PS7 * P[10][19] + P[3][19]; + nextP[4][19] = -PS171 * P[15][19] + PS172 * P[14][19] + PS173 * P[1][19] + PS174 * P[0][19] + PS175 * P[2][19] - PS176 * P[3][19] + PS43 * P[13][19] + P[4][19]; + nextP[5][19] = PS190 * P[15][19] - PS193 * P[13][19] + PS201 * P[2][19] - PS202 * P[0][19] + PS203 * P[3][19] - PS204 * P[1][19] + PS75 * P[14][19] + P[5][19]; + nextP[6][19] = -PS197 * P[14][19] + PS199 * P[13][19] - PS214 * P[2][19] + PS215 * P[3][19] + PS216 * P[0][19] + PS217 * P[1][19] + PS87 * P[15][19] + P[6][19]; + nextP[7][19] = P[4][19] * cov_dt + P[7][19]; + nextP[8][19] = P[5][19] * cov_dt + P[8][19]; + nextP[9][19] = P[6][19] * cov_dt + P[9][19]; + nextP[10][19] = P[10][19]; + nextP[11][19] = P[11][19]; + nextP[12][19] = P[12][19]; + nextP[13][19] = P[13][19]; + nextP[14][19] = P[14][19]; + nextP[15][19] = P[15][19]; + nextP[16][19] = P[16][19]; + nextP[17][19] = P[17][19]; + nextP[18][19] = P[18][19]; + nextP[19][19] = P[19][19]; + nextP[0][20] = -PS11 * P[1][20] - PS12 * P[2][20] - PS13 * P[3][20] + PS6 * P[10][20] + PS7 * P[11][20] + PS9 * P[12][20] + P[0][20]; + nextP[1][20] = PS11 * P[0][20] - PS12 * P[3][20] + PS13 * P[2][20] - PS34 * P[10][20] - PS7 * P[12][20] + PS9 * P[11][20] + P[1][20]; + nextP[2][20] = PS11 * P[3][20] + PS12 * P[0][20] - PS13 * P[1][20] - PS34 * P[11][20] + PS6 * P[12][20] - PS9 * P[10][20] + P[2][20]; + nextP[3][20] = -PS11 * P[2][20] + PS12 * P[1][20] + PS13 * P[0][20] - PS34 * P[12][20] - PS6 * P[11][20] + PS7 * P[10][20] + P[3][20]; + nextP[4][20] = -PS171 * P[15][20] + PS172 * P[14][20] + PS173 * P[1][20] + PS174 * P[0][20] + PS175 * P[2][20] - PS176 * P[3][20] + PS43 * P[13][20] + P[4][20]; + nextP[5][20] = PS190 * P[15][20] - PS193 * P[13][20] + PS201 * P[2][20] - PS202 * P[0][20] + PS203 * P[3][20] - PS204 * P[1][20] + PS75 * P[14][20] + P[5][20]; + nextP[6][20] = -PS197 * P[14][20] + PS199 * P[13][20] - PS214 * P[2][20] + PS215 * P[3][20] + PS216 * P[0][20] + PS217 * P[1][20] + PS87 * P[15][20] + P[6][20]; + nextP[7][20] = P[4][20] * cov_dt + P[7][20]; + nextP[8][20] = P[5][20] * cov_dt + P[8][20]; + nextP[9][20] = P[6][20] * cov_dt + P[9][20]; + nextP[10][20] = P[10][20]; + nextP[11][20] = P[11][20]; + nextP[12][20] = P[12][20]; + nextP[13][20] = P[13][20]; + nextP[14][20] = P[14][20]; + nextP[15][20] = P[15][20]; + nextP[16][20] = P[16][20]; + nextP[17][20] = P[17][20]; + nextP[18][20] = P[18][20]; + nextP[19][20] = P[19][20]; + nextP[20][20] = P[20][20]; + nextP[0][21] = -PS11 * P[1][21] - PS12 * P[2][21] - PS13 * P[3][21] + PS6 * P[10][21] + PS7 * P[11][21] + PS9 * P[12][21] + P[0][21]; + nextP[1][21] = PS11 * P[0][21] - PS12 * P[3][21] + PS13 * P[2][21] - PS34 * P[10][21] - PS7 * P[12][21] + PS9 * P[11][21] + P[1][21]; + nextP[2][21] = PS11 * P[3][21] + PS12 * P[0][21] - PS13 * P[1][21] - PS34 * P[11][21] + PS6 * P[12][21] - PS9 * P[10][21] + P[2][21]; + nextP[3][21] = -PS11 * P[2][21] + PS12 * P[1][21] + PS13 * P[0][21] - PS34 * P[12][21] - PS6 * P[11][21] + PS7 * P[10][21] + P[3][21]; + nextP[4][21] = -PS171 * P[15][21] + PS172 * P[14][21] + PS173 * P[1][21] + PS174 * P[0][21] + PS175 * P[2][21] - PS176 * P[3][21] + PS43 * P[13][21] + P[4][21]; + nextP[5][21] = PS190 * P[15][21] - PS193 * P[13][21] + PS201 * P[2][21] - PS202 * P[0][21] + PS203 * P[3][21] - PS204 * P[1][21] + PS75 * P[14][21] + P[5][21]; + nextP[6][21] = -PS197 * P[14][21] + PS199 * P[13][21] - PS214 * P[2][21] + PS215 * P[3][21] + PS216 * P[0][21] + PS217 * P[1][21] + PS87 * P[15][21] + P[6][21]; + nextP[7][21] = P[4][21] * cov_dt + P[7][21]; + nextP[8][21] = P[5][21] * cov_dt + P[8][21]; + nextP[9][21] = P[6][21] * cov_dt + P[9][21]; + nextP[10][21] = P[10][21]; + nextP[11][21] = P[11][21]; + nextP[12][21] = P[12][21]; + nextP[13][21] = P[13][21]; + nextP[14][21] = P[14][21]; + nextP[15][21] = P[15][21]; + nextP[16][21] = P[16][21]; + nextP[17][21] = P[17][21]; + nextP[18][21] = P[18][21]; + nextP[19][21] = P[19][21]; + nextP[20][21] = P[20][21]; + nextP[21][21] = P[21][21]; + + for (uint8_t i = 10; i < NumStates; i++) + { + nextP[i][i] = nextP[i][i] + processNoiseVariance[i - 10]; + } + + if ((P[7][7] + P[8][8]) > 1e4f) + { + for (uint8_t i = 7; i <= 8; i++) + { + for (uint8_t j = 0; j < NumStates; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + for (uint8_t row = 0; row < NumStates; row++) + { + // copy diagonals + P[row][row] = nextP[row][row]; + // copy off diagonals + for (uint8_t column = 0; column < row; column++) + { + P[row][column] = P[column][row] = nextP[column][row]; + } + } + + ConstrainVariances(); + + calcTiltErrorVariance(); + +} + +void EKFilter::ConstrainVariances() +{ + for (uint8_t i = 0; i <= 3; i++) P[i][i] = constrain_value(P[i][i], 0.0, 1.0); // attitude error + for (uint8_t i = 4; i <= 5; i++) P[i][i] = constrain_value(P[i][i], 1e-4, 1.0e3); // NE velocity + + if (P[6][6] < 1e-4) + { + P[6][6] = 1e-4; + } + + float dtEkfAvg = 0.5 * (recentGyro.dt + recentAccel.dt); + + for (uint8_t i = 7; i <= 9; i++) P[i][i] = constrain_value(P[i][i], 1E-4, 1.0e6); + + for (uint8_t i = 10; i <= 12; i++) P[i][i] = constrain_value(P[i][i], 0.0f, sq(0.175 * dtEkfAvg)); + //for (uint8_t i = 10; i <= 12; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.005); + + const float minSafeStateVar = 5E-9; + float maxStateVar = 0.0F; + bool resetRequired = false; + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) + { + if (P[stateIndex][stateIndex] > maxStateVar) + { + maxStateVar = P[stateIndex][stateIndex]; + } + else if (P[stateIndex][stateIndex] < minSafeStateVar) + { + resetRequired = true; + } + } + // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must + // not exceed 100 and the minimum variance must not fall below the target minimum + float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minSafeStateVar); + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) + { + P[stateIndex][stateIndex] = constrain_value(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg)); + } + + // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero + if (resetRequired) + { + // reset all delta velocity bias covariances + zeroCols(P, 13, 15); + zeroRows(P, 13, 15); + // set all delta velocity bias variances to initial values and zero bias states + P[13][13] = sq(0.2f * _accBiasLim * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + State.accel_bias = { 0.0, 0.0, 0.0 }; + } + + //for (uint8_t i = 16; i <= 18; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.01f); // earth magnetic field + //for (uint8_t i = 19; i <= 21; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.01f); // body magnetic field + + for (uint8_t i = 16; i <= 18; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.5f); // earth magnetic field + for (uint8_t i = 19; i <= 21; i++) P[i][i] = constrain_value(P[i][i], 0.0f, 0.5f); // body magnetic field +} + +void EKFilter::calcTiltErrorVariance() +{ + const float& q0 = State.quat(0); + const float& q1 = State.quat(1); + const float& q2 = State.quat(2); + const float& q3 = State.quat(3); + + const float PS1 = q0 * q1 + q2 * q3; + const float PS2 = q1 * PS1; + const float PS4 = sq(q0) - sq(q1) - sq(q2) + sq(q3); + const float PS5 = q0 * PS4; + const float PS6 = 2 * PS2 + PS5; + const float PS8 = PS1 * q2; + const float PS10 = PS4 * q3; + const float PS11 = PS10 + 2 * PS8; + const float PS12 = PS1 * q3; + const float PS13 = PS4 * q2; + const float PS14 = -2 * PS12 + PS13; + const float PS15 = PS1 * q0; + const float PS16 = q1 * PS4; + const float PS17 = 2 * PS15 - PS16; + const float PS18 = q0 * q2 - q1 * q3; + const float PS19 = PS18 * q2; + const float PS20 = 2 * PS19 + PS5; + const float PS22 = q1 * PS18; + const float PS23 = -PS10 + 2 * PS22; + const float PS25 = PS18 * q3; + const float PS26 = PS16 + 2 * PS25; + const float PS28 = PS18 * q0; + const float PS29 = -PS13 + 2 * PS28; + const float PS32 = PS12 + PS28; + const float PS33 = PS19 + PS2; + const float PS34 = PS15 - PS25; + const float PS35 = PS22 - PS8; + + tiltErrorVariance = 4 * sq(PS11) * P[2][2] + 4 * sq(PS14) * P[3][3] + 4 * sq(PS17) * P[0][0] + 4 * sq(PS6) * P[1][1]; + tiltErrorVariance += 4 * sq(PS20) * P[2][2] + 4 * sq(PS23) * P[1][1] + 4 * sq(PS26) * P[3][3] + 4 * sq(PS29) * P[0][0]; + tiltErrorVariance += 16 * sq(PS32) * P[1][1] + 16 * sq(PS33) * P[3][3] + 16 * sq(PS34) * P[2][2] + 16 * sq(PS35) * P[0][0]; + + tiltErrorVariance = constrain_value(tiltErrorVariance, 0.0f, sq(radians(30.0f))); +} + +void EKFilter::ForceSymmetry() +{ + for (uint8_t i = 1; i < NumStates; i++) + { + for (uint8_t j = 0; j <= i - 1; j++) + { + float temp = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = temp; + P[j][i] = temp; + } + } +} + + diff --git a/Source/LuckyWorld/EKFilter.h b/Source/LuckyWorld/EKFilter.h new file mode 100644 index 00000000..e5095ca3 --- /dev/null +++ b/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& 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* GyroDataBuffer; + TArray* AccelDataBuffer; + TArray* MagDataBuffer; + TArray* GPSDataBuffer; + TArray* 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(fval) >= FLT_EPSILON); + } + + bool readIMUData(); + bool GetNextData(TArray* 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); +}; diff --git a/Source/LuckyWorld/FQuaternion.cpp b/Source/LuckyWorld/FQuaternion.cpp new file mode 100644 index 00000000..05887153 --- /dev/null +++ b/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(PI/2); + } + if (v <= -1.0f) { + return static_cast(-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; +} diff --git a/Source/LuckyWorld/FQuaternion.h b/Source/LuckyWorld/FQuaternion.h new file mode 100644 index 00000000..cbd85178 --- /dev/null +++ b/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); +}; diff --git a/Source/LuckyWorld/GPSSensor.cpp b/Source/LuckyWorld/GPSSensor.cpp new file mode 100644 index 00000000..a2b7aae0 --- /dev/null +++ b/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); +} + diff --git a/Source/LuckyWorld/GPSSensor.h b/Source/LuckyWorld/GPSSensor.h new file mode 100644 index 00000000..2cf29646 --- /dev/null +++ b/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 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; +}; diff --git a/Source/LuckyWorld/IMUSensor.cpp b/Source/LuckyWorld/IMUSensor.cpp new file mode 100644 index 00000000..eed0fd41 --- /dev/null +++ b/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); +} + diff --git a/Source/LuckyWorld/IMUSensor.h b/Source/LuckyWorld/IMUSensor.h new file mode 100644 index 00000000..f5fd7230 --- /dev/null +++ b/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 GyroDataBuffer; + + TArray AccelDataBuffer; + + TArray 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; +}; diff --git a/Source/LuckyWorld/Location.cpp b/Source/LuckyWorld/Location.cpp new file mode 100644 index 00000000..41a9a75e --- /dev/null +++ b/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; +} diff --git a/Source/LuckyWorld/Location.h b/Source/LuckyWorld/Location.h new file mode 100644 index 00000000..747095bc --- /dev/null +++ b/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(); +}; diff --git a/Source/LuckyWorld/Matrix33.cpp b/Source/LuckyWorld/Matrix33.cpp new file mode 100644 index 00000000..6c9f0c72 --- /dev/null +++ b/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); +} diff --git a/Source/LuckyWorld/Matrix33.h b/Source/LuckyWorld/Matrix33.h new file mode 100644 index 00000000..30feaabc --- /dev/null +++ b/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; +}; diff --git a/Source/LuckyWorld/MultiRotorDrone.cpp b/Source/LuckyWorld/MultiRotorDrone.cpp new file mode 100644 index 00000000..244dbb99 --- /dev/null +++ b/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 +#include +#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(); +} + + + + + + diff --git a/Source/LuckyWorld/MultiRotorDrone.h b/Source/LuckyWorld/MultiRotorDrone.h new file mode 100644 index 00000000..24b53392 --- /dev/null +++ b/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 RPM = { 0, 0, 0, 0 }; + + UPROPERTY(BlueprintReadWrite) + TArray ForcesToApply = { 0, 0, 0, 0 }; + + UPROPERTY(BlueprintReadWrite) + TArray MomentsToApply = { 0, 0, 0, 0 }; + + UPROPERTY(BlueprintReadWrite) + TArray ComputedDrag = { 0, 0, 0 }; + + UPROPERTY(VisibleAnywhere, BlueprintReadWrite, meta = (AllowPrivateAccess = true)) + TArray 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 cThrust = { 0.2, 0.1, 0.3, 0.09, 0.4, 0.075, 0.5, 0.055, 0.6, 0.04 }; + TArray 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 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; + } + +}; diff --git a/Source/LuckyWorld/Random.cpp b/Source/LuckyWorld/Random.cpp new file mode 100644 index 00000000..9709c6af --- /dev/null +++ b/Source/LuckyWorld/Random.cpp @@ -0,0 +1,55 @@ +// Fill out your copyright notice in the Description page of Project Settings. + + +#include "Random.h" +#include +#include + +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 dist(min, max); + return dist(generator); + //return 0; +} diff --git a/Source/LuckyWorld/Random.h b/Source/LuckyWorld/Random.h new file mode 100644 index 00000000..3a482768 --- /dev/null +++ b/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 + +/** + * + */ +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; +};