forked from Archive/PX4-Autopilot
prediction and vel pos heading fusion working
This commit is contained in:
Roman
Roman Bapst
parent
921df43d32
commit
8de8b0eb76
|
|
@ -46,5 +46,7 @@ px4_add_module(
|
|||
EKF/ekf.cpp
|
||||
EKF/ekf_helper.cpp
|
||||
EKF/covariance.cpp
|
||||
EKF/vel_pos_fusion.cpp
|
||||
EKF/heading_fusion.cpp
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
|
|
|
|||
|
|
@ -42,37 +42,38 @@
|
|||
|
||||
#include "ekf.h"
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define sq(_arg) powf(_arg, 2.0f)
|
||||
|
||||
void Ekf::initialiseCovariance()
|
||||
{
|
||||
for (unsigned i = 0; i < _kNumStates; i++) {
|
||||
for (unsigned j = 0; j < _kNumStates; j++) {
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
for (unsigned j = 0; j < _k_num_states; j++) {
|
||||
P[i][j] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// XXX use initial guess for the diagonal elements for the covariance matrix
|
||||
// angle error
|
||||
P[0][0] = 0.1f;
|
||||
P[1][1] = 0.1f;
|
||||
P[2][2] = 0.1f;
|
||||
P[0][0] = 0.001f;
|
||||
P[1][1] = 0.001f;
|
||||
P[2][2] = 0.001f;
|
||||
|
||||
// velocity
|
||||
P[3][3] = 9.0f;
|
||||
P[4][4] = 9.0f;
|
||||
P[5][5] = 9.0f;
|
||||
P[3][3] = 0.1f;
|
||||
P[4][4] = 0.1f;
|
||||
P[5][5] = 0.1f;
|
||||
|
||||
// position
|
||||
P[6][6] = 9.0f;
|
||||
P[7][7] = 9.0f;
|
||||
P[8][8] = 9.0f;
|
||||
P[6][6] = 0.1f;
|
||||
P[7][7] = 0.1f;
|
||||
P[8][8] = 0.1f;
|
||||
|
||||
// gyro bias
|
||||
P[9][9] = 0.001f;
|
||||
P[10][10] = 0.001f;
|
||||
P[11][11] = 0.001f;
|
||||
P[9][9] = 0.00001f;
|
||||
P[10][10] = 0.00001f;
|
||||
P[11][11] = 0.00001f;
|
||||
|
||||
// gyro scale
|
||||
P[12][12] = 0.0001f;
|
||||
|
|
@ -93,8 +94,8 @@ void Ekf::initialiseCovariance()
|
|||
P[21][21] = 0.0001f;
|
||||
|
||||
// wind
|
||||
P[22][22] = 4.0f;
|
||||
P[23][23] = 4.0f;
|
||||
P[22][22] = 0.01f;
|
||||
P[23][23] = 0.01f;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -124,28 +125,50 @@ void Ekf::predictCovariance()
|
|||
|
||||
float dvz_b = _state.accel_z_bias;
|
||||
|
||||
float dt = _imu_sample_delayed.delta_ang_dt;
|
||||
float dt = _imu_sample_delayed.delta_vel_dt;
|
||||
|
||||
// compute process noise
|
||||
float process_noise[_k_num_states] = {};
|
||||
|
||||
float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
|
||||
float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 1e-6f, 1e-2f);
|
||||
float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 1e-6f, 1e-2f);
|
||||
float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
|
||||
float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
|
||||
float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.01f, 1.0f);
|
||||
|
||||
for (unsigned i = 0; i < 9; i++) {
|
||||
process_noise[i] = 0.0f;
|
||||
}
|
||||
for (unsigned i = 9; i < 12; i++) {
|
||||
process_noise[i] = sq(d_ang_bias_sig);
|
||||
}
|
||||
for (unsigned i = 12; i < 15; i++) {
|
||||
process_noise[i] = sq(d_ang_scale_sig);
|
||||
}
|
||||
process_noise[15] = sq(d_vel_bias_sig);
|
||||
|
||||
for (unsigned i = 16; i < 19; i++) {
|
||||
process_noise[i] = sq(mag_I_sig);
|
||||
}
|
||||
for (unsigned i = 19; i < 22; i++) {
|
||||
process_noise[i] = sq(mag_B_sig);
|
||||
}
|
||||
for (unsigned i = 22; i < 24; i++) {
|
||||
process_noise[i] = sq(wind_vel_sig);
|
||||
}
|
||||
|
||||
|
||||
// assign input noise
|
||||
// inputs to the system are 3 delta angles and 3 delta velocities
|
||||
float daxNoise = _params.dax_noise;
|
||||
float dayNoise = _params.day_noise;
|
||||
float dazNoise = _params.daz_noise;
|
||||
float daxNoise, dayNoise, dazNoise;
|
||||
float dvxNoise, dvyNoise, dvzNoise;
|
||||
float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-2f);
|
||||
daxNoise = dayNoise = dazNoise = dt * gyro_noise;
|
||||
float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f);
|
||||
dvxNoise = dvyNoise = dvzNoise = dt * accel_noise;
|
||||
|
||||
float dvxNoise = _params.dvx_noise;
|
||||
float dvyNoise = _params.dvy_noise;
|
||||
float dvzNoise = _params.dvz_noise;
|
||||
|
||||
// assign process noise variables
|
||||
float delta_ang_sig = _params.delta_ang_sig;
|
||||
float delta_vel_sig = _params.delta_vel_sig;
|
||||
float delta_pos_sig = _params.delta_pos_sig;
|
||||
float delta_gyro_bias_sig = _params.delta_gyro_bias_sig;
|
||||
float delta_gyro_scale_sig = _params.delta_gyro_scale_sig;
|
||||
float delta_vel_bias_z_sig = _params.delta_vel_bias_z_sig;
|
||||
float delta_mag_body_sig = _params.delta_mag_body_sig;
|
||||
float delta_mag_earth_sig = _params.delta_mag_earth_sig;
|
||||
float delta_wind_sig = _params.delta_wind_sig;
|
||||
// predict covarinace matrix
|
||||
|
||||
// intermediate calculations
|
||||
float SF[25] = {};
|
||||
|
|
@ -632,35 +655,104 @@ void Ekf::predictCovariance()
|
|||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
|
||||
// add process noise to diagonal
|
||||
for (int i = 0; i < 3; i++) {nextP[i][i] += delta_ang_sig * delta_ang_sig;}
|
||||
// add process noise
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
nextP[i][i] += process_noise[i];
|
||||
}
|
||||
|
||||
for (int i = 3; i < 6; i++) {nextP[i][i] += delta_vel_sig * delta_vel_sig;}
|
||||
|
||||
for (int i = 6; i < 9; i++) {nextP[i][i] += delta_vel_sig * delta_pos_sig;}
|
||||
|
||||
for (int i = 9; i < 12; i++) {nextP[i][i] += delta_gyro_bias_sig * delta_gyro_bias_sig;}
|
||||
|
||||
for (int i = 12; i < 15; i++) {nextP[i][i] += delta_gyro_scale_sig * delta_gyro_scale_sig;}
|
||||
|
||||
nextP[15][15] += delta_vel_bias_z_sig * delta_vel_bias_z_sig;
|
||||
|
||||
for (int i = 16; i < 19; i++) {nextP[i][i] += delta_mag_earth_sig * delta_mag_earth_sig;}
|
||||
|
||||
for (int i = 19; i < 22; i++) {nextP[i][i] += delta_mag_body_sig * delta_mag_body_sig;}
|
||||
|
||||
for (int i = 19; i < 24; i++) {nextP[i][i] += delta_wind_sig * delta_wind_sig;}
|
||||
// stop position covariance growth if our total position variance reaches 100m
|
||||
// this can happen if we loose gps for some time
|
||||
if ((P[6][6] + P[7][7]) > 1e4f) {
|
||||
for (uint8_t i = 6; i < 8; i++) {
|
||||
for (uint8_t j = 0; j < _k_num_states; j++) {
|
||||
nextP[i][j] = P[i][j];
|
||||
nextP[j][i] = P[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
for (unsigned row = 1; row < _kNumStates; row++) {
|
||||
for (unsigned row = 1; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0 ; column < row; column++) {
|
||||
nextP[row][column] = nextP[column][row];
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _kNumStates; row++) {
|
||||
for (unsigned column = 0; column < _kNumStates; column++) {
|
||||
P[row][column] = nextP[row][column];
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
for (unsigned row = 1; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]);
|
||||
P[column][row] = P[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
limitCov();
|
||||
}
|
||||
|
||||
void Ekf::limitCov()
|
||||
{
|
||||
// Covariance diagonal limits. Use same values for states which
|
||||
// belong to the same group (e.g. vel_x, vel_y, vel_z)
|
||||
float P_lim[9] = {};
|
||||
P_lim[0] = 1.0f; // angle error max var
|
||||
P_lim[1] = 1000.0f; // velocity max var
|
||||
P_lim[2] = 1000000.0f; // positiion max var
|
||||
P_lim[3] = 0.001f; // gyro bias max var
|
||||
P_lim[4] = 0.01f; // gyro scale max var
|
||||
P_lim[5] = 0.1f; // delta velocity z bias max var
|
||||
P_lim[6] = 0.01f; // earth mag field max var
|
||||
P_lim[7] = 0.01f; // body mag field max var
|
||||
P_lim[8] = 1000.0f; // wind max var
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[0]);
|
||||
}
|
||||
|
||||
for (int i = 3; i < 6; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[1]);
|
||||
}
|
||||
|
||||
for (int i = 6; i < 9; i++) {
|
||||
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[2]);
|
||||
}
|
||||
|
||||
for (int i = 9; i < 12; i++) {
|
||||
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[3]);
|
||||
}
|
||||
|
||||
for (int i = 12; i < 15; i++) {
|
||||
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[4]);
|
||||
}
|
||||
|
||||
|
||||
math::constrain(P[15][15], 0.0f, P_lim[5]);
|
||||
|
||||
for (int i = 16; i < 19; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[6]);
|
||||
}
|
||||
|
||||
for (int i = 19; i < 22; i++) {
|
||||
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[7]);
|
||||
}
|
||||
|
||||
for (int i = 22; i < 24; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[8]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
109
EKF/ekf.cpp
109
EKF/ekf.cpp
|
|
@ -42,9 +42,15 @@
|
|||
#include "ekf.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
Ekf::Ekf()
|
||||
Ekf::Ekf():
|
||||
_filter_initialised(false),
|
||||
_earth_rate_initialised(false),
|
||||
_fuse_height(false),
|
||||
_fuse_pos(false),
|
||||
_fuse_vel(false)
|
||||
{
|
||||
|
||||
_earth_rate_NED.setZero();
|
||||
_R_prev = matrix::Dcm<float>();
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -53,22 +59,24 @@ Ekf::~Ekf()
|
|||
|
||||
}
|
||||
|
||||
void Ekf::update()
|
||||
bool Ekf::update()
|
||||
{
|
||||
bool ret = false; // indicates if there has been an update
|
||||
if (!_filter_initialised) {
|
||||
_filter_initialised = initialiseFilter();
|
||||
}
|
||||
|
||||
printStates();
|
||||
// prediction
|
||||
if (_imu_updated) {
|
||||
ret = true;
|
||||
predictState();
|
||||
predictCovariance();
|
||||
_imu_updated = false;
|
||||
}
|
||||
|
||||
// measurement updates
|
||||
|
||||
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
|
||||
fuseMag();
|
||||
fuseHeading();
|
||||
}
|
||||
|
||||
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
|
||||
|
|
@ -80,8 +88,10 @@ void Ekf::update()
|
|||
_fuse_vel = true;
|
||||
}
|
||||
|
||||
|
||||
if (_fuse_height || _fuse_pos || _fuse_vel) {
|
||||
fusePosVel();
|
||||
fuseVelPosHeight();
|
||||
_fuse_vel = _fuse_pos = _fuse_height = false;
|
||||
}
|
||||
|
||||
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
|
||||
|
|
@ -92,11 +102,20 @@ void Ekf::update()
|
|||
fuseAirspeed();
|
||||
}
|
||||
|
||||
// write to output if this has been a prediction step
|
||||
if (_imu_updated) {
|
||||
_output_delayed.vel = _state.vel;
|
||||
_output_delayed.pos = _state.pos;
|
||||
_output_delayed.quat_nominal = _state.quat_nominal;
|
||||
_output_delayed.time_us = _imu_time_last;
|
||||
_imu_updated = false;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool Ekf::initialiseFilter(void)
|
||||
{
|
||||
|
||||
_state.ang_error.setZero();
|
||||
_state.vel.setZero();
|
||||
_state.pos.setZero();
|
||||
|
|
@ -133,39 +152,36 @@ bool Ekf::initialiseFilter(void)
|
|||
|
||||
void Ekf::predictState()
|
||||
{
|
||||
// compute transformation matrix from body to world frame
|
||||
matrix::Dcm<float> R(_state.quat_nominal);
|
||||
R.transpose();
|
||||
if (!_earth_rate_initialised) {
|
||||
if (_gps_initialised) {
|
||||
calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad );
|
||||
_earth_rate_initialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
// attitude error state prediciton
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(_imu_sample_delayed.delta_ang);
|
||||
matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame
|
||||
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
|
||||
Quaternion dq; // delta quaternion since last update
|
||||
dq.from_axis_angle(corrected_delta_ang);
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
_R_prev = R_to_earth.transpose();
|
||||
|
||||
Vector3f vel_last = _state.vel;
|
||||
|
||||
// predict velocity states
|
||||
_state.vel += R * _imu_sample_delayed.delta_vel;
|
||||
_state.vel += R_to_earth * _imu_sample_delayed.delta_vel;
|
||||
_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;
|
||||
|
||||
// predict position states via trapezoidal integration of velocity
|
||||
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
|
||||
|
||||
//matrix::Euler<float> euler(_state.quat_nominal);
|
||||
//printf("roll pitch yaw %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
|
||||
constrainStates();
|
||||
}
|
||||
|
||||
|
||||
void Ekf::fusePosVel()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void Ekf::fuseMag()
|
||||
{
|
||||
}
|
||||
|
||||
void Ekf::fuseAirspeed()
|
||||
{
|
||||
|
||||
|
|
@ -175,3 +191,46 @@ void Ekf::fuseRange()
|
|||
{
|
||||
|
||||
}
|
||||
|
||||
void Ekf::printStates()
|
||||
{
|
||||
static int counter = 0;
|
||||
|
||||
if (counter % 50 == 0) {
|
||||
printf("ang error\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("ang_e %i %.5f\n", i, (double)_state.ang_error(i));
|
||||
}
|
||||
|
||||
matrix::Euler<float> euler(_state.quat_nominal);
|
||||
printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
|
||||
|
||||
printf("vel\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("v %i %.5f\n", i, (double)_state.vel(i));
|
||||
}
|
||||
|
||||
printf("pos\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("p %i %.5f\n", i, (double)_state.pos(i));
|
||||
}
|
||||
|
||||
printf("g gyro_bias\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("gb %i %.5f\n", i, (double)_state.gyro_bias(i));
|
||||
}
|
||||
|
||||
printf("gyro_scale\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
|
||||
}
|
||||
|
||||
printf("mag_I\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
|
||||
}
|
||||
counter = 0;
|
||||
}
|
||||
counter++;
|
||||
|
||||
}
|
||||
|
|
|
|||
39
EKF/ekf.h
39
EKF/ekf.h
|
|
@ -41,6 +41,8 @@
|
|||
|
||||
#include "estimator_base.h"
|
||||
|
||||
#define sq(_arg) powf(_arg, 2.0f)
|
||||
|
||||
class Ekf : public EstimatorBase
|
||||
{
|
||||
public:
|
||||
|
|
@ -48,18 +50,25 @@ public:
|
|||
Ekf();
|
||||
~Ekf();
|
||||
|
||||
void update();
|
||||
bool update();
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t _kNumStates = 24;
|
||||
static const uint8_t _k_num_states = 24;
|
||||
static constexpr float _k_earth_rate = 0.000072921f;
|
||||
|
||||
bool _filter_initialised;
|
||||
bool _earth_rate_initialised;
|
||||
|
||||
bool _fuse_height; // true if there is new baro data
|
||||
bool _fuse_pos; // true if there is new position data from gps
|
||||
bool _fuse_vel; // true if there is new velocity data from gps
|
||||
|
||||
float P[_kNumStates][_kNumStates]; // state covariance matrix
|
||||
Vector3f _earth_rate_NED;
|
||||
|
||||
matrix::Dcm<float> _R_prev;
|
||||
|
||||
float P[_k_num_states][_k_num_states]; // state covariance matrix
|
||||
|
||||
bool initialiseFilter(void);
|
||||
|
||||
|
|
@ -69,17 +78,37 @@ private:
|
|||
|
||||
void predictCovariance();
|
||||
|
||||
void fusePosVel();
|
||||
void fuseMag(uint8_t index);
|
||||
|
||||
void fuseMag();
|
||||
void fuseHeading();
|
||||
|
||||
void fuseAirspeed();
|
||||
|
||||
void fuseRange();
|
||||
|
||||
void fuseVelPosHeight();
|
||||
|
||||
void resetVelocity();
|
||||
|
||||
void resetPosition();
|
||||
|
||||
void makeCovSymetrical();
|
||||
|
||||
void limitCov();
|
||||
|
||||
void printCovToFile(char const *filename);
|
||||
|
||||
void assertCovNiceness();
|
||||
|
||||
void makeSymmetrical();
|
||||
|
||||
void constrainStates();
|
||||
|
||||
void fuse(float *K, float innovation);
|
||||
|
||||
void printStates();
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
|
||||
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -40,7 +40,13 @@
|
|||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
// Reset the velocity states. If we have a recent and valid
|
||||
// gps measurement then use for velocity initialisation
|
||||
void Ekf::resetVelocity()
|
||||
{
|
||||
// if we have a valid GPS measurement use it to initialise velocity states
|
||||
|
|
@ -54,6 +60,8 @@ void Ekf::resetVelocity()
|
|||
}
|
||||
}
|
||||
|
||||
// Reset position states. If we have a recent and valid
|
||||
// gps measurement then use for position initialisation
|
||||
void Ekf::resetPosition()
|
||||
{
|
||||
// if we have a valid GPS measurement use it to initialise position states
|
||||
|
|
@ -70,3 +78,91 @@ void Ekf::resetPosition()
|
|||
baroSample baro_newest = _baro_buffer.get_newest();
|
||||
_state.pos(2) = baro_newest.hgt;
|
||||
}
|
||||
|
||||
void Ekf::printCovToFile(char const *filename)
|
||||
{
|
||||
std::ofstream myfile;
|
||||
myfile.open(filename);
|
||||
myfile << "Covariance matrix\n";
|
||||
myfile << std::setprecision(1);
|
||||
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
for (int j = 0; j < _k_num_states; j++) {
|
||||
myfile << std::to_string(P[i][j]) << std::setprecision(1) << " ";
|
||||
}
|
||||
|
||||
myfile << "\n\n\n\n\n\n\n\n\n\n";
|
||||
}
|
||||
}
|
||||
|
||||
// This checks if the diagonal of the covariance matrix is non-negative
|
||||
// and that the matrix is symmetric
|
||||
void Ekf::assertCovNiceness()
|
||||
{
|
||||
for (int row = 0; row < _k_num_states; row++) {
|
||||
for (int column = 0; column < row; column++) {
|
||||
assert(fabsf(P[row][column] - P[column][row]) < 0.00001f);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
assert(P[i][i] > -0.000001f);
|
||||
}
|
||||
}
|
||||
|
||||
// This function forces the covariance matrix to be symmetric
|
||||
void Ekf::makeSymmetrical()
|
||||
{
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
float tmp = (P[row][column] + P[column][row]) / 2;
|
||||
P[row][column] = tmp;
|
||||
P[column][row] = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::constrainStates()
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.ang_error(i) = math::constrain(_state.ang_error(i), -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.vel(i) = math::constrain(_state.vel(i), -1000.0f, 1000.0f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.pos(i) = math::constrain(_state.pos(i), -1.e6f, 1.e6f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_imu_avg, 0.349066f * _dt_imu_avg);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.gyro_scale(i) = math::constrain(_state.gyro_scale(i), 0.95f, 1.05f);
|
||||
}
|
||||
|
||||
_state.accel_z_bias = math::constrain(_state.accel_z_bias, -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the earth rotation vector
|
||||
void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const
|
||||
{
|
||||
omega(0) = _k_earth_rate * cosf((float)lat_rad);
|
||||
omega(1) = 0.0f;
|
||||
omega(2) = -_k_earth_rate * sinf((float)lat_rad);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ EstimatorBase::~EstimatorBase()
|
|||
|
||||
}
|
||||
|
||||
// read integrated imu data and store to buffer
|
||||
// Accumulate imu data and store to buffer at desired rate
|
||||
void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang,
|
||||
float *delta_vel)
|
||||
{
|
||||
|
|
@ -132,7 +132,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
|
|||
_imu_sample_delayed = _imu_buffer.get_oldest();
|
||||
}
|
||||
|
||||
|
||||
void EstimatorBase::setMagData(uint64_t time_usec, float *data)
|
||||
{
|
||||
|
||||
|
|
@ -161,11 +160,9 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
|
|||
return;
|
||||
}
|
||||
|
||||
|
||||
if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) {
|
||||
|
||||
gpsSample gps_sample_new = {};
|
||||
gps_sample_new.time_us = time_usec - _params.gps_delay_ms * 1000;
|
||||
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
|
||||
|
||||
|
||||
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
|
||||
|
|
@ -260,25 +257,24 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
|
|||
|
||||
_params.mag_delay_ms = 0;
|
||||
_params.baro_delay_ms = 0;
|
||||
_params.gps_delay_ms = 0;
|
||||
_params.gps_delay_ms = 200;
|
||||
_params.airspeed_delay_ms = 0;
|
||||
_params.requiredEph = 10;
|
||||
_params.requiredEpv = 10;
|
||||
_params.dax_noise = 0.0f;
|
||||
_params.day_noise = 0.0f;
|
||||
_params.daz_noise = 0.0f;
|
||||
_params.dvx_noise = 0.0f;
|
||||
_params.dvy_noise = 0.0f;
|
||||
_params.dvz_noise = 0.0f;
|
||||
_params.delta_ang_sig = 0.0f;
|
||||
_params.delta_vel_sig = 0.0f;
|
||||
_params.delta_pos_sig = 0.0f;
|
||||
_params.delta_gyro_bias_sig = 0.0f;
|
||||
_params.delta_gyro_scale_sig = 0.0f;
|
||||
_params.delta_vel_bias_z_sig = 0.0f;
|
||||
_params.delta_mag_body_sig = 0.0f;
|
||||
_params.delta_mag_earth_sig = 0.0f;
|
||||
_params.delta_wind_sig = 0.0f;
|
||||
_params.requiredEph = 200;
|
||||
_params.requiredEpv = 200;
|
||||
_params.gyro_noise = 1e-3f;
|
||||
_params.accel_noise = 1e-1f;
|
||||
_params.gyro_bias_p_noise = 1e-5f;
|
||||
_params.accel_bias_p_noise = 1e-3f;
|
||||
_params.gyro_scale_p_noise = 1e-4f;
|
||||
_params.mag_p_noise = 1e-2f;
|
||||
_params.wind_vel_p_noise = 0.05f;
|
||||
|
||||
_params.gps_vel_noise = 0.05f;
|
||||
_params.gps_pos_noise = 1.0f;
|
||||
_params.baro_noise = 0.1f;
|
||||
_params.mag_heading_noise = 3e-2f;
|
||||
_params.mag_declination_deg = 10.0f;
|
||||
_params.heading_innov_gate = 0.5f;
|
||||
|
||||
_dt_imu_avg = 0.0f;
|
||||
_imu_time_last = time_usec;
|
||||
|
|
@ -308,6 +304,9 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
|
|||
_gps_initialised = false;
|
||||
_gps_speed_valid = false;
|
||||
|
||||
_mag_healthy = false;
|
||||
_in_air = true; // XXX get this flag from the application
|
||||
|
||||
_time_last_imu = 0;
|
||||
_time_last_gps = 0;
|
||||
_time_last_mag = 0;
|
||||
|
|
@ -315,6 +314,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
|
|||
_time_last_range = 0;
|
||||
_time_last_airspeed = 0;
|
||||
|
||||
memset(&_fault_status, 0, sizeof(_fault_status));
|
||||
|
||||
}
|
||||
|
||||
void EstimatorBase::initialiseGPS(struct gps_message *gps)
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ public:
|
|||
EstimatorBase();
|
||||
~EstimatorBase();
|
||||
|
||||
virtual void update() = 0;
|
||||
virtual bool update() = 0;
|
||||
|
||||
// set delta angle imu data
|
||||
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
|
||||
|
|
@ -111,6 +111,7 @@ protected:
|
|||
Quaternion quat_nominal;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct imuSample {
|
||||
|
|
@ -118,40 +119,40 @@ protected:
|
|||
Vector3f delta_vel;
|
||||
float delta_ang_dt;
|
||||
float delta_vel_dt;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct gpsSample {
|
||||
Vector2f pos;
|
||||
float hgt;
|
||||
Vector3f vel;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct magSample {
|
||||
Vector3f mag;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct baroSample {
|
||||
float hgt;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct rangeSample {
|
||||
float rng;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct airspeedSample {
|
||||
float airspeed;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct flowSample {
|
||||
Vector2f flowRadXY;
|
||||
Vector2f flowRadXYcomp;
|
||||
uint32_t time_us;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct {
|
||||
|
|
@ -162,31 +163,32 @@ protected:
|
|||
float requiredEph;
|
||||
float requiredEpv;
|
||||
|
||||
float dax_noise;
|
||||
float day_noise;
|
||||
float daz_noise;
|
||||
float dvx_noise;
|
||||
float dvy_noise;
|
||||
float dvz_noise;
|
||||
float gyro_noise;
|
||||
float accel_noise;
|
||||
|
||||
float delta_ang_sig;
|
||||
float delta_vel_sig;
|
||||
float delta_pos_sig;
|
||||
float delta_gyro_bias_sig;
|
||||
float delta_gyro_scale_sig;
|
||||
float delta_vel_bias_z_sig;
|
||||
float delta_mag_body_sig;
|
||||
float delta_mag_earth_sig;
|
||||
float delta_wind_sig;
|
||||
// process noise
|
||||
float gyro_bias_p_noise;
|
||||
float accel_bias_p_noise;
|
||||
float gyro_scale_p_noise;
|
||||
float mag_p_noise;
|
||||
float wind_vel_p_noise;
|
||||
|
||||
float gps_vel_noise;
|
||||
float gps_pos_noise;
|
||||
float baro_noise;
|
||||
|
||||
float mag_heading_noise; // measurement noise used for simple heading fusion
|
||||
float mag_declination_deg; // magnetic declination in degrees
|
||||
float heading_innov_gate; // innovation gate for heading innovation test
|
||||
|
||||
} _params;
|
||||
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 5;
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 25;
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 10;
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 30;
|
||||
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10;
|
||||
|
||||
float _dt_imu_avg;
|
||||
uint32_t _imu_time_last;
|
||||
uint64_t _imu_time_last;
|
||||
|
||||
imuSample _imu_sample_delayed;
|
||||
imuSample _imu_down_sampled;
|
||||
|
|
@ -200,11 +202,13 @@ protected:
|
|||
airspeedSample _airspeed_sample_delayed;
|
||||
flowSample _flow_sample_delayed;
|
||||
|
||||
outputSample _output_delayed;
|
||||
|
||||
struct map_projection_reference_s _posRef;
|
||||
float _gps_alt_ref;
|
||||
|
||||
|
||||
uint32_t _imu_ticks;
|
||||
uint64_t _imu_ticks;
|
||||
|
||||
bool _imu_updated;
|
||||
bool _start_predict_enabled;
|
||||
|
|
@ -212,6 +216,10 @@ protected:
|
|||
bool _gps_initialised;
|
||||
bool _gps_speed_valid;
|
||||
|
||||
bool _mag_healthy; // computed by mag innovation test
|
||||
|
||||
bool _in_air; // indicates if the vehicle is in the air
|
||||
|
||||
RingBuffer<imuSample> _imu_buffer;
|
||||
RingBuffer<gpsSample> _gps_buffer;
|
||||
RingBuffer<magSample> _mag_buffer;
|
||||
|
|
@ -228,6 +236,15 @@ protected:
|
|||
uint64_t _time_last_range;
|
||||
uint64_t _time_last_airspeed;
|
||||
|
||||
// flags capturing information about severe nummerical problems for various fusions
|
||||
struct {
|
||||
bool bad_mag_x:1;
|
||||
bool bad_mag_y:1;
|
||||
bool bad_mag_z:1;
|
||||
bool bad_airspeed:1;
|
||||
bool bad_sideslip:1;
|
||||
} _fault_status;
|
||||
|
||||
|
||||
void initialiseVariables(uint64_t timestamp);
|
||||
|
||||
|
|
@ -246,4 +263,23 @@ public:
|
|||
void printStoredBaro();
|
||||
void printGps(struct gpsSample *data);
|
||||
void printStoredGps();
|
||||
|
||||
void copy_quaternion(float *quat) {
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
quat[i] = _state.quat_nominal(i);
|
||||
}
|
||||
}
|
||||
void copy_velocity(float *vel) {
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
vel[i] = _state.vel(i);
|
||||
}
|
||||
}
|
||||
void copy_position(float *pos) {
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
pos[i] = _state.pos(i);
|
||||
}
|
||||
}
|
||||
void copy_timestamp(uint64_t *time_us) {
|
||||
*time_us = _imu_time_last;
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,182 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name ECL nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file heading_fusion.cpp
|
||||
* Magnetometer heading fusion.
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
*
|
||||
*/
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::fuseHeading()
|
||||
{
|
||||
// assign intermediate state variables
|
||||
float q0 = _state.quat_nominal(0);
|
||||
float q1 = _state.quat_nominal(1);
|
||||
float q2 = _state.quat_nominal(2);
|
||||
float q3 = _state.quat_nominal(3);
|
||||
|
||||
float magX = _mag_sample_delayed.mag(0);
|
||||
float magY = _mag_sample_delayed.mag(1);
|
||||
float magZ = _mag_sample_delayed.mag(2);
|
||||
|
||||
float R_mag = _params.mag_heading_noise;
|
||||
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = q0*q2*2.0f;
|
||||
float t7 = q1*q3*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q3*2.0f;
|
||||
float t13 = q1*q2*2.0f;
|
||||
float t10 = t9-t13;
|
||||
float t11 = t2+t3-t4-t5;
|
||||
float t12 = magX*t11;
|
||||
float t14 = magZ*t8;
|
||||
float t19 = magY*t10;
|
||||
float t15 = t12+t14-t19;
|
||||
float t16 = t2-t3+t4-t5;
|
||||
float t17 = q0*q1*2.0f;
|
||||
float t24 = q2*q3*2.0f;
|
||||
float t18 = t17-t24;
|
||||
float t20 = 1.0f/t15;
|
||||
float t21 = magY*t16;
|
||||
float t22 = t9+t13;
|
||||
float t23 = magX*t22;
|
||||
float t28 = magZ*t18;
|
||||
float t25 = t21+t23-t28;
|
||||
float t29 = t20*t25;
|
||||
float t26 = tan(t29);
|
||||
float t27 = 1.0f/(t15*t15);
|
||||
float t30 = t26*t26;
|
||||
float t31 = t30+1.0f;
|
||||
|
||||
float H_MAG[3] = {};
|
||||
H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
|
||||
H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
|
||||
H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
|
||||
|
||||
// calculate innovation
|
||||
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
|
||||
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
|
||||
float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - math::radians(_params.mag_declination_deg);
|
||||
|
||||
innovation = math::constrain(innovation, -0.5f, 0.5f);
|
||||
|
||||
float innovation_var = R_mag;
|
||||
|
||||
// calculate innovation variance
|
||||
float PH[3] = {};
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
PH[row] += P[row][column]*H_MAG[column];
|
||||
}
|
||||
innovation_var += H_MAG[row] * PH[row];
|
||||
}
|
||||
|
||||
if (innovation_var >= R_mag) {
|
||||
// variance has increased, no failure
|
||||
_fault_status.bad_mag_x = false;
|
||||
_fault_status.bad_mag_y = false;
|
||||
_fault_status.bad_mag_z = false;
|
||||
} else {
|
||||
// our innovation variance has decreased, our calculation is thus badly conditioned
|
||||
_fault_status.bad_mag_x = true;
|
||||
_fault_status.bad_mag_y = true;
|
||||
_fault_status.bad_mag_z = true;
|
||||
|
||||
// we reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
float innovation_var_inv = 1 / innovation_var;
|
||||
|
||||
// calculate kalman gain
|
||||
float Kfusion[_k_num_states] = {};
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
Kfusion[row] += P[row][column] * H_MAG[column];
|
||||
}
|
||||
Kfusion[row] *= innovation_var_inv;
|
||||
}
|
||||
|
||||
// innovation test ratio
|
||||
float yaw_test_ratio = sq(innovation) / (sq(math::max(0.01f * (float)_params.heading_innov_gate, 1.0f)) * innovation_var);
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (yaw_test_ratio > 1.0f) {
|
||||
_mag_healthy = false;
|
||||
|
||||
// if we are in air we don't want to fuse the measurement
|
||||
// we allow to use it when on the ground because the large innovation could be caused
|
||||
// by interference or a large initial gyro bias
|
||||
if (_in_air) {
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
_mag_healthy = true;
|
||||
}
|
||||
|
||||
_state.ang_error.setZero();
|
||||
fuse(Kfusion, innovation);
|
||||
|
||||
// correct the nominal quaternion
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
float HP[_k_num_states] = {};
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
HP[column] += H_MAG[row] * P[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] -= Kfusion[row] * HP[column];
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
|
|
@ -0,0 +1,162 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name ECL nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vel_pos_fusion.cpp
|
||||
* Function for fusing gps and baro measurements/
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::fuseVelPosHeight()
|
||||
{
|
||||
bool fuse_map[6] = {};
|
||||
float innovations[6] = {};
|
||||
float R[6] = {};
|
||||
float Kfusion[24] = {};
|
||||
// calculate innovations
|
||||
if (_fuse_vel) {
|
||||
fuse_map[0] = fuse_map[1] = fuse_map[2] = true;
|
||||
innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
|
||||
innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
|
||||
innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
|
||||
R[0] = _params.gps_vel_noise;
|
||||
R[1] = _params.gps_vel_noise;
|
||||
R[2] = _params.gps_vel_noise;
|
||||
}
|
||||
|
||||
if (_fuse_pos) {
|
||||
fuse_map[3] = fuse_map[4] = true;
|
||||
innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
R[3] = _params.gps_pos_noise;
|
||||
R[4] = _params.gps_pos_noise;
|
||||
|
||||
}
|
||||
|
||||
if (_fuse_height) {
|
||||
fuse_map[5] = true;
|
||||
innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis
|
||||
R[5] = _params.baro_noise;
|
||||
}
|
||||
|
||||
// XXX Do checks here
|
||||
|
||||
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
|
||||
if (!fuse_map[obs_index]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
|
||||
|
||||
// compute the innovation variance SK = HPH + R
|
||||
float S = P[state_index][state_index] + R[obs_index];
|
||||
S = 1.0f / S;
|
||||
|
||||
// calculate kalman gain K = PHS
|
||||
for (int row = 0; row < 24; row++) {
|
||||
Kfusion[row] = P[row][state_index] * S;
|
||||
}
|
||||
|
||||
// by definition the angle error state is zero at the fusion time
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// fuse the observation
|
||||
fuse(Kfusion, innovations[obs_index]);
|
||||
|
||||
// correct the nominal quaternion
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
// update covarinace matrix via Pnew = (I - KH)P
|
||||
float KHP[_k_num_states][_k_num_states] = {};
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
KHP[row][column] = Kfusion[row] * P[state_index][column];
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ekf::fuse(float *K, float innovation)
|
||||
{
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
|
||||
}
|
||||
|
||||
_state.accel_z_bias -= K[15] * innovation;
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 2; i++) {
|
||||
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue