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

Make flow_innov/-var a matrix Vector2f

Browse Source
This commit is contained in:
kamilritz
2020-06-26 08:26:26 +02:00
committed by Mathieu Bresciani
parent c2801eb488
commit 5356077a32
4 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+10 -10
View File
@@ -231,27 +231,27 @@ void Ekf::fuseFlowForTerrain()
_terrain_var = fmaxf(_terrain_var, 0.0f);
// Cacluate innovation variance
_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;
_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
// calculate the kalman gain for the flow x measurement
const float Kx = _terrain_var * Hx / _flow_innov_var[0];
const float Kx = _terrain_var * Hx / _flow_innov_var(0);
// calculate prediced optical flow about x axis
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (x axis)
_flow_innov[0] = pred_flow_x - opt_flow_rate(0);
_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
// calculate correction term for terrain variance
const float KxHxP = Kx * Hx * _terrain_var;
// innovation consistency check
const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);
float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
// do not perform measurement update if badly conditioned
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Kx * _flow_innov[0];
_terrain_vpos += Kx * _flow_innov(0);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
_time_last_of_fuse = _time_last_imu;
@@ -261,25 +261,25 @@ void Ekf::fuseFlowForTerrain()
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
// Calculuate innovation variance
_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;
_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
// calculate the kalman gain for the flow y measurement
const float Ky = _terrain_var * Hy / _flow_innov_var[1];
const float Ky = _terrain_var * Hy / _flow_innov_var(1);
// calculate prediced optical flow about y axis
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (y axis)
_flow_innov[1] = pred_flow_y - opt_flow_rate(1);
_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
// calculate correction term for terrain variance
const float KyHyP = Ky * Hy * _terrain_var;
// innovation consistency check
flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);
flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Ky * _flow_innov[1];
_terrain_vpos += Ky * _flow_innov(1);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
_time_last_of_fuse = _time_last_imu;