From e7dc665c1479d45eaf37e94a4f7808ec68839dc1 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 1 May 2013 19:16:29 +0200 Subject: [PATCH] Reworked Variance Objects, Setting for initial EKF variance --- flight/modules/Attitude/revolution/attitude.c | 23 +++++++------- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ .../ekfinitialvariance.xml | 30 +++++++++++++++++++ .../uavobjectdefinition/ekfstatevariance.xml | 18 ++++++++++- 6 files changed, 61 insertions(+), 14 deletions(-) create mode 100644 shared/uavobjectdefinition/ekfinitialvariance.xml diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 899622d5d..c4a5b51d1 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -66,6 +66,7 @@ #include "homelocation.h" #include "magnetometer.h" #include "positionactual.h" +#include "ekfinitialvariance.h" #include "ekfstatevariance.h" #include "revocalibration.h" #include "revosettings.h" @@ -159,6 +160,7 @@ int32_t AttitudeInitialize(void) VelocityActualInitialize(); RevoSettingsInitialize(); RevoCalibrationInitialize(); + EKFInitialVarianceInitialize(); EKFStateVarianceInitialize(); // Initialize this here while we aren't setting the homelocation in GPS @@ -755,14 +757,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // Don't initialize until all sensors are read if (init_stage == 0) { - float Pdiag[13]={ - 25.0f, 25.0f, 25.0f, // initial position variance - 5 meters mean uncertainty (5²=25) - 5.0f, 5.0f, 5.0f, // initial velocity variance - 2.2 m/s mean uncertainty - 7e-3f, 7e-3f, 7e-3f, 7e-3f, // initial orientation variance - 5 deg (sin(5°)² ~ 7e-3) - 1e-4f, 1e-4f, 1e-4f, // initial gyro drift variance - 0.6 deg/s (sin(0.6°)² ~ 1e-4) - }; - float q[4]; - float pos[3] = {0.0f, 0.0f, 0.0f}; // Reset the INS algorithm INSGPSInit(); @@ -776,6 +770,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) float gyro_bias[3] = {0,0,0}; INSSetGyroBias(gyro_bias); + float pos[3] = {0.0f, 0.0f, 0.0f}; + if (outdoor_mode) { GPSPositionData gpsPosition; @@ -827,12 +823,13 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); - q[0] = attitudeActual.q1; - q[1] = attitudeActual.q2; - q[2] = attitudeActual.q3; - q[3] = attitudeActual.q4; + float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 }; INSSetState(pos, zeros, q, zeros, zeros); - INSResetP(Pdiag); + + EKFInitialVarianceData vardata; + EKFInitialVarianceGet(&vardata); + INSResetP(vardata.P); + } else { // Run prediction a bit before any corrections diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index ce4ce26e0..79618f517 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -68,6 +68,7 @@ UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings +UAVOBJSRCFILENAMES += ekfinitialvariance UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revosettings diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index c4dfd035d..063ae4590 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -73,6 +73,7 @@ UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += relaytuning UAVOBJSRCFILENAMES += relaytuningsettings +UAVOBJSRCFILENAMES += ekfinitialvariance UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += revocalibration UAVOBJSRCFILENAMES += revosettings diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 7ead99fe3..6437bb4f6 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -33,6 +33,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/ekfinitialvariance.h \ $$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \ $$UAVOBJECT_SYNTHETICS/revosettings.h \ @@ -113,6 +114,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/ekfinitialvariance.cpp \ $$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ $$UAVOBJECT_SYNTHETICS/revosettings.cpp \ diff --git a/shared/uavobjectdefinition/ekfinitialvariance.xml b/shared/uavobjectdefinition/ekfinitialvariance.xml new file mode 100644 index 000000000..6dc0bdf75 --- /dev/null +++ b/shared/uavobjectdefinition/ekfinitialvariance.xml @@ -0,0 +1,30 @@ + + + Extended Kalman Filter covariance initialisation + + + PositionNorth + PositionEast + PositionDown + VelocityNorth + VelocityEast + VelocityDown + AttitudeQ1 + AttitudeQ2 + AttitudeQ3 + AttitudeQ4 + GyroDriftX + GyroDriftY + GyroDriftZ + + + + + + + + diff --git a/shared/uavobjectdefinition/ekfstatevariance.xml b/shared/uavobjectdefinition/ekfstatevariance.xml index 16a47e50d..05b684dbd 100644 --- a/shared/uavobjectdefinition/ekfstatevariance.xml +++ b/shared/uavobjectdefinition/ekfstatevariance.xml @@ -1,7 +1,23 @@ Extended Kalman Filter state covariance - + + + PositionNorth + PositionEast + PositionDown + VelocityNorth + VelocityEast + VelocityDown + AttitudeQ1 + AttitudeQ2 + AttitudeQ3 + AttitudeQ4 + GyroDriftX + GyroDriftY + GyroDriftZ + +