rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Reworked Variance Objects, Setting for initial EKF variance

Browse Source
This commit is contained in:
Corvus Corax 2013-05-01 19:16:29 +02:00
parent 3ef2693dfc
commit e7dc665c14
6 changed files with 61 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/modules/Attitude/revolution/attitude.c
View File

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

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -68,6 +68,7 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfinitialvariance
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -73,6 +73,7 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfinitialvariance
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

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

30
shared/uavobjectdefinition/ekfinitialvariance.xml Normal file
View File

@ -0,0 +1,30 @@
<xml>
<object name="EKFInitialVariance" singleinstance="true" settings="true">
<description>Extended Kalman Filter covariance initialisation</description>
<field name="P" units="1^2" type="float" defaultvalue="
25.0, 25.0, 25.0,
5.0, 5.0, 5.0,
0.007, 0.007, 0.007, 0.007,
0.0001, 0.0001, 0.0001">
<elementnames>
<elementname>PositionNorth</elementname>
<elementname>PositionEast</elementname>
<elementname>PositionDown</elementname>
<elementname>VelocityNorth</elementname>
<elementname>VelocityEast</elementname>
<elementname>VelocityDown</elementname>
<elementname>AttitudeQ1</elementname>
<elementname>AttitudeQ2</elementname>
<elementname>AttitudeQ3</elementname>
<elementname>AttitudeQ4</elementname>
<elementname>GyroDriftX</elementname>
<elementname>GyroDriftY</elementname>
<elementname>GyroDriftZ</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

18
shared/uavobjectdefinition/ekfstatevariance.xml
View File

@ -1,7 +1,23 @@
<xml>
<object name="EKFStateVariance" singleinstance="true" settings="false">
<description>Extended Kalman Filter state covariance</description>
<field name="P" units="1^2" type="float" elements="13"/>
<field name="P" units="1^2" type="float">
<elementnames>
<elementname>PositionNorth</elementname>
<elementname>PositionEast</elementname>
<elementname>PositionDown</elementname>
<elementname>VelocityNorth</elementname>
<elementname>VelocityEast</elementname>
<elementname>VelocityDown</elementname>
<elementname>AttitudeQ1</elementname>
<elementname>AttitudeQ2</elementname>
<elementname>AttitudeQ3</elementname>
<elementname>AttitudeQ4</elementname>
<elementname>GyroDriftX</elementname>
<elementname>GyroDriftY</elementname>
<elementname>GyroDriftZ</elementname>
</elementnames>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/>