fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 07:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

- do not fake vertical gps measurement as we have baro

Browse Source 
- formatting

Conflicts:
	EKF/ekf.cpp
This commit is contained in:
Roman
2016-01-08 06:59:10 +01:00
parent 2a88fc6cfd
commit f8354bb5e9
3 changed files with 150 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+84 -79
View File
@@ -41,64 +41,69 @@
#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;
}
void Ekf::fuseVelPosHeight()
{
bool fuse_map[6] = {};
float innovations[6] = {};
float R[6] = {};
float Kfusion[24] = {};
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;
// calculate innovations
if (_fuse_hor_vel) {
fuse_map[0] = fuse_map[1] = true;
innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
R[0] = _params.gps_vel_noise;
R[1] = _params.gps_vel_noise;
}
}
if (_fuse_vert_vel) {
fuse_map[2] = true;
innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
R[2] = _params.gps_vel_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;
}
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;
// XXX Do checks here
}
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (!fuse_map[obs_index]) {
continue;
}
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;
}
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
// XXX Do checks here
// compute the innovation variance SK = HPH + R
float S = P[state_index][state_index] + R[obs_index];
S = 1.0f / S;
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (!fuse_map[obs_index]) {
continue;
}
// 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();
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
// fuse the observation
fuse(Kfusion, innovations[obs_index]);
// compute the innovation variance SK = HPH + R
float S = P[state_index][state_index] + R[obs_index];
S = 1.0f / S;
// correct the nominal quaternion
Quaternion dq;
// 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();
@@ -120,43 +125,43 @@
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;
}
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.pos(i) = _state.pos(i) - K[i + 6] * 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.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * 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_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
}
_state.accel_z_bias -= K[15] * innovation;
for (unsigned i = 0; i < 3; i++) {
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
}
_state.accel_z_bias -= K[15] * 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 < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
}
for (unsigned i = 0; i < 2; i++) {
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * 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;
}
}