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 @@
+
+
+
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 @@