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

ekf2: terrain flow - migrate to Symforce

Browse Source
This commit is contained in:
bresch
2023-01-21 15:31:19 -05:00
committed by Daniel Agar
parent 3f842f01a0
commit b4b48cae75
5 changed files with 277 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/terrain_estimator.cpp
+55 -72
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
* 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
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -33,13 +33,13 @@
/**
* @file terrain_estimator.cpp
* Function for fusing rangefinder measurements to estimate terrain vertical position/
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
* Function for fusing rangefinder and optical flow measurements
* to estimate terrain vertical position
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
#include <mathlib/mathlib.h>
@@ -254,6 +254,8 @@ void Ekf::controlHaglFlowFusion()
}
if (_flow_data_ready) {
updateOptFlow(_aid_src_optical_flow);
const bool continuing_conditions_passing = _control_status.flags.in_air
&& !_control_status.flags.opt_flow
&& _control_status.flags.gps
@@ -320,90 +322,71 @@ void Ekf::fuseFlowForTerrain()
{
_aid_src_optical_flow.fusion_enabled = true;
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
// get latest estimated orientation
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
float range = predictFlowRange();
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
const float state = _terrain_vpos; // linearize both axes using the same state value
Vector2f innov_var;
float H;
sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
// get rotation matrix from earth to body
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor relative to the imu in body frame
// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
// calculate the velocity of the sensor in the earth frame
const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
// rotate into body frame
const Vector3f vel_body = earth_to_body * vel_rel_earth;
// constrain terrain to minimum allowed value and predict height above ground
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
// calculate prediced optical flow
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation
_aid_src_optical_flow.innovation[0] = pred_flow_x - opt_flow_rate(0);
_aid_src_optical_flow.innovation[1] = pred_flow_y - opt_flow_rate(1);
const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
// Calculate observation matrix for flow around
const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
// Constrain terrain variance to be non-negative
_terrain_var = fmaxf(_terrain_var, sq(0.01f));
// Cacluate innovation variance
_aid_src_optical_flow.innovation_variance[0] = Hx * Hx * _terrain_var + R_LOS;
_aid_src_optical_flow.innovation_variance[1] = Hy * Hy * _terrain_var + R_LOS;
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|| (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
return;
}
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
// do not perform measurement update if badly conditioned
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
// if either axis fails we abort the fusion
if (_aid_src_optical_flow.innovation_rejected) {
return;
}
// calculate the kalman gain for the flow measurement
const float Kx = _terrain_var * Hx / _aid_src_optical_flow.innovation_variance[0];
// fuse observation axes sequentially
for (uint8_t index = 0; index <= 1; index++) {
if (index == 0) {
// everything was already computed above
// calculate correction term for terrain variance
const float KxHxP = Kx * Hx * _terrain_var;
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
_terrain_vpos += Kx * _aid_src_optical_flow.innovation[0];
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, sq(0.01f));
// recalculate the innovation using the updated state
const Vector2f vel_body = predictFlowVelBody();
range = predictFlowRange();
_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
_terrain_var = 100.0f;
return;
}
}
// calculate the kalman gain for the flow measurement
const float Ky = _terrain_var * Hy / _aid_src_optical_flow.innovation_variance[1];
float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index];
// calculate correction term for terrain variance
const float KyHyP = Ky * Hy * _terrain_var;
_terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0];
// constrain terrain to minimum allowed value and predict height above ground
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
_terrain_vpos += Ky * _aid_src_optical_flow.innovation_variance[1];
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f));
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
}
_fault_status.flags.bad_optflow_X = false;
_fault_status.flags.bad_optflow_Y = false;
_time_last_flow_terrain_fuse = _time_delayed_us;
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?