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

Expand auto-format coverage and tiny style changes

Browse Source
This commit is contained in:
kamilritz
2020-01-27 16:54:25 +01:00
committed by Mathieu Bresciani
parent fbdd75da2e
commit f20726d47f
12 changed files with 178 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+38 -43
View File
@@ -41,39 +41,37 @@
*
*/
#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
#include "ekf.h"
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(4,4) + obs_var(0);
innov_var(1) = P(5,5) + obs_var(1);
innov_var(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass)
{
if (innov_check_pass) {
_time_last_hor_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hor_vel = false;
fuseVelPosHeight(innov(0),innov_var(0),0);
fuseVelPosHeight(innov(1),innov_var(1),1);
fuseVelPosHeight(innov(0), innov_var(0), 0);
fuseVelPosHeight(innov(1), innov_var(1), 1);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_vel = true;
return false;
}
}
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(6,6) + obs_var(2);
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f);
@@ -81,20 +79,20 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
_time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false;
fuseVelPosHeight(innov(2),innov_var(2),2);
fuseVelPosHeight(innov(2), innov_var(2), 2);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_vel = true;
return false;
}
}
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7,7) + obs_var(0);
innov_var(1) = P(8,8) + obs_var(1);
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(0) = P(7, 7) + obs_var(0);
innov_var(1) = P(8, 8) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
@@ -108,20 +106,20 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0),innov_var(0),3);
fuseVelPosHeight(innov(1),innov_var(1),4);
fuseVelPosHeight(innov(0), innov_var(0), 3);
fuseVelPosHeight(innov(1), innov_var(1), 4);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_pos = true;
return false;
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(9,9) + obs_var(2);
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
@@ -129,31 +127,31 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innov(2),innov_var(2),5);
fuseVelPosHeight(innov(2), innov_var(2), 5);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_pos = true;
return false;
}
}
// Helper function that fuses a single velocity or position measurement
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) {
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < _k_num_states; row++) {
Kfusion[row] = P(row,state_index) / innov_var;
Kfusion[row] = P(row, state_index) / innov_var;
}
matrix::SquareMatrix<float, _k_num_states> KHP;
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);
KHP(row, column) = Kfusion[row] * P(state_index, column);
}
}
@@ -162,7 +160,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
if (P(i,i) < KHP(i,i)) {
if (P(i, i) < KHP(i, i)) {
// zero rows and columns
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
@@ -175,13 +173,12 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
}
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
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);
P(row, column) = P(row, column) - KHP(row, column);
}
}
@@ -190,12 +187,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
// apply the state corrections
fuse(Kfusion, innov);
}
}
void Ekf::setVelPosFaultStatus(const int index, const bool status)
{
void Ekf::setVelPosFaultStatus(const int index, const bool status) {
if (index == 0) {
_fault_status.flags.bad_vel_N = status;