dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Numerical checks on covariances

This commit is contained in:
Lorenz Meier 2014-03-18 09:19:57 +01:00
parent 37513eaafa
commit 03ccee289b
3 changed files with 184 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
src/modules/fw_att_pos_estimator/estimator.cpp
View File

@ -1,5 +1,7 @@
#include "estimator.h"
#include <string.h>
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
@ -67,9 +69,21 @@ bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying)
bool useAirspeed = true; // boolean true if airspeed data is being used
bool useCompass = true; // boolean true if magnetometer data is being used
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode = true; ///< boolean true if no position feedback is fused
bool useAirspeed = true; ///< boolean true if airspeed data is being used
bool useCompass = true; ///< boolean true if magnetometer data is being used
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
bool numericalProtection = true;
static unsigned storeIndex = 0;
float Vector3f::length(void) const
{
@ -889,7 +903,7 @@ void CovariancePrediction(float dt)
{
for (uint8_t i=7; i<=8; i++)
{
for (uint8_t j=0; j<=20; j++)
for (unsigned j = 0; j < n_states; j++)
{
nextP[i][j] = P[i][j];
nextP[j][i] = P[j][i];
@ -897,19 +911,39 @@ void CovariancePrediction(float dt)
}
}
// Force symmetry on the covariance matrix to prevent ill-conditioning
// of the matrix which would cause the filter to blow-up
for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i];
for (uint8_t i=1; i<=20; i++)
{
for (uint8_t j=0; j<=i-1; j++)
{
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
if (onGround || staticMode) {
// copy the portion of the variances we want to
// propagate
for (unsigned i = 0; i < 14; i++) {
P[i][i] = nextP[i][i];
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
if ((i > 12) || (j > 12)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}
}
} else {
// Copy covariance
for (unsigned i = 0; i < n_states; i++) {
P[i][i] = nextP[i][i];
}
ForceSymmetry();
}
//
ConstrainVariances();
}
void FuseVelposNED()
@ -923,12 +957,6 @@ void FuseVelposNED()
static uint32_t velFailTime = 0;
static uint32_t posFailTime = 0;
static uint32_t hgtFailTime = 0;
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
// declare variables used to check measurement errors
float velInnov[3] = {0.0f,0.0f,0.0f};
@ -1137,6 +1165,9 @@ void FuseVelposNED()
}
}
ForceSymmetry();
ConstrainVariances();
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
}
@ -1442,6 +1473,9 @@ void FuseMagnetometer()
}
}
obsIndex = obsIndex + 1;
ForceSymmetry();
ConstrainVariances();
}
void FuseAirspeed()
@ -1568,6 +1602,9 @@ void FuseAirspeed()
}
}
}
ForceSymmetry();
ConstrainVariances();
}
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@ -1604,8 +1641,6 @@ float sq(float valIn)
// Store states in a history array along with time stamp
void StoreStates(uint64_t timestamp_ms)
{
/* static to keep the store index */
static uint8_t storeIndex = 0;
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = timestamp_ms;
@ -1614,6 +1649,26 @@ void StoreStates(uint64_t timestamp_ms)
storeIndex = 0;
}
void ResetStates()
{
// reset all stored states
memset(&storedStates[0][0], 0, sizeof(storedStates));
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
// reset store index to first
storeIndex = 0;
// overwrite all existing states
for (unsigned i = 0; i < n_states; i++) {
storedStates[i][storeIndex] = states[i];
}
statetimeStamp[storeIndex] = millis();
// increment to next storage index
storeIndex++;
}
// Output the state vector stored at the time that best matches that specified by msec
void RecallStates(float statesForFusion[n_states], uint64_t msec)
{
@ -1740,6 +1795,9 @@ void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef,
void OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
staticMode = !(GPSstatus > GPS_FIX_2D);
}
}
void calcEarthRateNED(Vector3f &omega, float latitude)
@ -1783,6 +1841,10 @@ float ConstrainFloat(float val, float min, float max)
void ConstrainVariances()
{
if (!numericalProtection) {
return;
}
// State vector:
// 0-3: quaternions (q0, q1, q2, q3)
// 4-6: Velocity - m/sec (North, East, Down)
@ -1831,6 +1893,10 @@ void ConstrainVariances()
void ConstrainStates()
{
if (!numericalProtection) {
return;
}
// State vector:
// 0-3: quaternions (q0, q1, q2, q3)
// 4-6: Velocity - m/sec (North, East, Down)
@ -1882,6 +1948,88 @@ void ConstrainStates()
}
void ForceSymmetry()
{
if (!numericalProtection) {
return;
}
// Force symmetry on the covariance matrix to prevent ill-conditioning
// of the matrix which would cause the filter to blow-up
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
P[i][j] = 0.5f * (P[i][j] + P[j][i]);
P[j][i] = P[i][j];
}
}
}
bool FilterHealthy()
{
if (!statesInitialised) {
return false;
}
// XXX Check state vector for NaNs and ill-conditioning
// Check if any of the major inputs timed out
if (posTimeout || velTimeout || hgtTimeout) {
return false;
}
// Nothing fired, return ok.
return true;
}
/**
* Reset the filter position states
*
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition(void)
{
if (staticMode) {
states[7] = 0;
states[8] = 0;
} else if (GPSstatus >= GPS_FIX_3D) {
// reset the states from the GPS measurements
states[7] = posNE[0];
states[8] = posNE[1];
}
}
/**
* Reset the height state.
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight(void)
{
// write to the state vector
states[9] = -hgtMea;
}
/**
* Reset the velocity state.
*/
void ResetVelocity(void)
{
if (staticMode) {
states[4] = 0.0f;
states[5] = 0.0f;
states[6] = 0.0f;
} else if (GPSstatus >= GPS_FIX_3D) {
states[4] = velNED[0]; // north velocity from last reading
states[5] = velNED[1]; // east velocity from last reading
states[6] = velNED[2]; // down velocity from last reading
}
}
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
{
float initialRoll, initialPitch;

11
src/modules/fw_att_pos_estimator/estimator.h
View File

@ -121,10 +121,19 @@ extern uint8_t GPSstatus;
extern float baroHgt;
extern bool statesInitialised;
extern bool numericalProtection;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
@ -175,5 +184,7 @@ void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
uint32_t millis();

5
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -1050,7 +1050,7 @@ void print_status()
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
printf("states: %s %s %s %s %s %s %s %s %s\n",
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(statesInitialised) ? "INITIALIZED" : "NON_INIT",
(onGround) ? "ON_GROUND" : "AIRBORNE",
(fuseVelData) ? "FUSE_VEL" : "INH_VEL",
@ -1059,7 +1059,8 @@ void print_status()
(fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS");
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
int fw_att_pos_estimator_main(int argc, char *argv[])