mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-09 01:21:35 +08:00
Daniel Agar
cea185268e
- update all msgs to be directly compatible with ROS2 - microdds_client improvements - timesync - reduced code size - add to most default builds if we can afford it - lots of other little changes - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)
413 lines
17 KiB
C++
413 lines
17 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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/
|
|
* equations generated using EKF/python/ekf_derivation/main.py
|
|
*
|
|
* @author Paul Riseborough <p_riseborough@live.com.au>
|
|
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
|
|
*
|
|
*/
|
|
|
|
#include "ekf.h"
|
|
|
|
#include <mathlib/mathlib.h>
|
|
#include <float.h>
|
|
#include "utils.hpp"
|
|
|
|
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
|
{
|
|
resetEstimatorAidStatus(aid_src);
|
|
aid_src.timestamp_sample = _flow_sample_delayed.time_us;
|
|
|
|
// 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;
|
|
|
|
// calculate the sensor position relative to the IMU in earth frame
|
|
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
|
|
|
// 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.
|
|
const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
|
|
|
// calculate range from focal point to centre of image
|
|
const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
|
|
|
|
// 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;
|
|
|
|
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
|
|
_flow_vel_body(0) = -opt_flow_rate(1) * range;
|
|
_flow_vel_body(1) = opt_flow_rate(0) * range;
|
|
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
|
|
|
|
aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
|
|
aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
|
|
|
|
aid_src.innovation[0] = (vel_body(1) / range) - aid_src.observation[0];
|
|
aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
|
|
|
|
// calculate the optical flow observation variance
|
|
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
|
|
aid_src.observation_variance[0] = R_LOS;
|
|
aid_src.observation_variance[1] = R_LOS;
|
|
}
|
|
|
|
void Ekf::fuseOptFlow()
|
|
{
|
|
_aid_src_optical_flow.fusion_enabled = true;
|
|
|
|
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);
|
|
|
|
// get latest velocity in earth frame
|
|
const float vn = _state.vel(0);
|
|
const float ve = _state.vel(1);
|
|
const float vd = _state.vel(2);
|
|
|
|
// 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.
|
|
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
|
|
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
|
const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
|
|
|
// calculate range from focal point to centre of image
|
|
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
|
const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
|
|
|
|
// The derivation allows for an arbitrary body to flow sensor frame rotation which is
|
|
// currently not supported by the EKF, so assume sensor frame is aligned with the
|
|
// body frame
|
|
const Dcmf Tbs = matrix::eye<float, 3>();
|
|
|
|
// Sub Expressions
|
|
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
|
|
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
|
|
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
|
|
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
|
|
const float HK4 = 1.0F/range;
|
|
const float HK5 = 2*HK4;
|
|
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
|
|
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
|
|
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
|
|
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
|
|
const float HK10 = q0*q2;
|
|
const float HK11 = q1*q3;
|
|
const float HK12 = HK10 + HK11;
|
|
const float HK13 = 2*Tbs(1,2);
|
|
const float HK14 = q0*q3;
|
|
const float HK15 = q1*q2;
|
|
const float HK16 = HK14 - HK15;
|
|
const float HK17 = 2*Tbs(1,1);
|
|
const float HK18 = ecl::powf(q1, 2);
|
|
const float HK19 = ecl::powf(q2, 2);
|
|
const float HK20 = -HK19;
|
|
const float HK21 = ecl::powf(q0, 2);
|
|
const float HK22 = ecl::powf(q3, 2);
|
|
const float HK23 = HK21 - HK22;
|
|
const float HK24 = HK18 + HK20 + HK23;
|
|
const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
|
|
const float HK26 = HK14 + HK15;
|
|
const float HK27 = 2*Tbs(1,0);
|
|
const float HK28 = q0*q1;
|
|
const float HK29 = q2*q3;
|
|
const float HK30 = HK28 - HK29;
|
|
const float HK31 = -HK18;
|
|
const float HK32 = HK19 + HK23 + HK31;
|
|
const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
|
|
const float HK34 = HK28 + HK29;
|
|
const float HK35 = HK10 - HK11;
|
|
const float HK36 = HK20 + HK21 + HK22 + HK31;
|
|
const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
|
|
const float HK38 = 2*HK3;
|
|
const float HK39 = 2*HK7;
|
|
const float HK40 = 2*HK8;
|
|
const float HK41 = 2*HK9;
|
|
const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
|
|
const float HK43 = ecl::powf(range, -2);
|
|
const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
|
|
const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
|
|
const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
|
|
const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
|
|
const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
|
|
const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
|
|
// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
|
|
|
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
|
_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
|
|
|
if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
|
|
// we need to reinitialise the covariance matrix and abort this fusion step
|
|
initialiseCovariance();
|
|
return;
|
|
}
|
|
const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
|
|
|
|
const float HK51 = Tbs(0,1)*q1;
|
|
const float HK52 = Tbs(0,2)*q0;
|
|
const float HK53 = Tbs(0,0)*q2;
|
|
const float HK54 = HK51 + HK52 - HK53;
|
|
const float HK55 = Tbs(0,0)*q3;
|
|
const float HK56 = Tbs(0,1)*q0;
|
|
const float HK57 = Tbs(0,2)*q1;
|
|
const float HK58 = HK55 + HK56 - HK57;
|
|
const float HK59 = Tbs(0,0)*q0;
|
|
const float HK60 = Tbs(0,2)*q2;
|
|
const float HK61 = Tbs(0,1)*q3;
|
|
const float HK62 = HK59 + HK60 - HK61;
|
|
const float HK63 = HK54*vd + HK58*ve + HK62*vn;
|
|
const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
|
|
const float HK65 = HK58*vd + HK64*vn;
|
|
const float HK66 = -HK54*ve + HK65;
|
|
const float HK67 = HK54*vn + HK64*ve;
|
|
const float HK68 = -HK62*vd + HK67;
|
|
const float HK69 = HK62*ve + HK64*vd;
|
|
const float HK70 = -HK58*vn + HK69;
|
|
const float HK71 = 2*Tbs(0,1);
|
|
const float HK72 = 2*Tbs(0,2);
|
|
const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
|
|
const float HK74 = -HK16*HK71 + HK73;
|
|
const float HK75 = 2*Tbs(0,0);
|
|
const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
|
|
const float HK77 = -HK30*HK72 + HK76;
|
|
const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
|
|
const float HK79 = -HK35*HK75 + HK78;
|
|
const float HK80 = 2*HK63;
|
|
const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
|
|
const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
|
|
const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
|
|
const float HK84 = HK71*(-HK14 + HK15) + HK73;
|
|
const float HK85 = HK72*(-HK28 + HK29) + HK76;
|
|
const float HK86 = HK75*(-HK10 + HK11) + HK78;
|
|
const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
|
|
const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
|
|
const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
|
|
const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
|
|
const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
|
|
const float HK92 = 2*HK43;
|
|
const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
|
|
const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
|
|
// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
|
|
|
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
|
_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
|
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
|
|
// we need to reinitialise the covariance matrix and abort this fusion step
|
|
initialiseCovariance();
|
|
return;
|
|
}
|
|
|
|
const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];
|
|
|
|
// run the innovation consistency check and record result
|
|
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
|
|
|
_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;
|
|
}
|
|
|
|
bool fused[2] {false, false};
|
|
|
|
// fuse observation axes sequentially
|
|
{
|
|
// Optical flow observation Jacobians - axis 0
|
|
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
|
Hfusion.at<0>() = HK3*HK5;
|
|
Hfusion.at<1>() = HK5*HK7;
|
|
Hfusion.at<2>() = HK5*HK8;
|
|
Hfusion.at<3>() = HK5*HK9;
|
|
Hfusion.at<4>() = HK25*HK4;
|
|
Hfusion.at<5>() = HK33*HK4;
|
|
Hfusion.at<6>() = HK37*HK4;
|
|
|
|
// Optical flow Kalman gains - axis 0
|
|
Vector24f Kfusion;
|
|
Kfusion(0) = HK42*HK50;
|
|
Kfusion(1) = HK49*HK50;
|
|
Kfusion(2) = HK47*HK50;
|
|
Kfusion(3) = HK48*HK50;
|
|
Kfusion(4) = HK46*HK50;
|
|
Kfusion(5) = HK45*HK50;
|
|
Kfusion(6) = HK44*HK50;
|
|
|
|
for (unsigned row = 7; row <= 23; row++) {
|
|
Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
|
|
}
|
|
|
|
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
|
|
fused[0] = true;
|
|
_fault_status.flags.bad_optflow_X = false;
|
|
|
|
} else {
|
|
_fault_status.flags.bad_optflow_X = true;
|
|
return;
|
|
}
|
|
}
|
|
|
|
{
|
|
// Optical flow observation Jacobians - axis 1
|
|
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
|
Hfusion.at<0>() = -HK5*HK63;
|
|
Hfusion.at<1>() = -HK5*HK66;
|
|
Hfusion.at<2>() = -HK5*HK68;
|
|
Hfusion.at<3>() = -HK5*HK70;
|
|
Hfusion.at<4>() = -HK4*HK74;
|
|
Hfusion.at<5>() = -HK4*HK77;
|
|
Hfusion.at<6>() = -HK4*HK79;
|
|
|
|
// Optical flow Kalman gains - axis 1
|
|
Vector24f Kfusion;
|
|
Kfusion(0) = -HK87*HK95;
|
|
Kfusion(1) = -HK94*HK95;
|
|
Kfusion(2) = -HK91*HK95;
|
|
Kfusion(3) = -HK93*HK95;
|
|
Kfusion(4) = -HK90*HK95;
|
|
Kfusion(5) = -HK89*HK95;
|
|
Kfusion(6) = -HK88*HK95;
|
|
|
|
for (unsigned row = 7; row <= 23; row++) {
|
|
Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
|
|
}
|
|
|
|
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
|
|
fused[1] = true;
|
|
_fault_status.flags.bad_optflow_Y = false;
|
|
|
|
} else {
|
|
_fault_status.flags.bad_optflow_Y = true;
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (fused[0] && fused[1]) {
|
|
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
|
|
_aid_src_optical_flow.fused = true;
|
|
}
|
|
}
|
|
|
|
// calculate optical flow body angular rate compensation
|
|
// returns false if bias corrected body rate data is unavailable
|
|
bool Ekf::calcOptFlowBodyRateComp()
|
|
{
|
|
bool is_body_rate_comp_available = false;
|
|
const bool use_flow_sensor_gyro = _flow_sample_delayed.gyro_xyz.isAllFinite();
|
|
|
|
if (use_flow_sensor_gyro) {
|
|
|
|
// if accumulation time differences are not excessive and accumulation time is adequate
|
|
// compare the optical flow and and navigation rate data and calculate a bias error
|
|
if ((_delta_time_of > FLT_EPSILON)
|
|
&& (_flow_sample_delayed.dt > FLT_EPSILON)
|
|
&& (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) {
|
|
|
|
const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
|
|
|
|
const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt));
|
|
|
|
// calculate the bias estimate using a combined LPF and spike filter
|
|
_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
|
|
|
|
// apply gyro bias
|
|
_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
|
|
|
|
is_body_rate_comp_available = true;
|
|
}
|
|
|
|
} else {
|
|
// Use the EKF gyro data if optical flow sensor gyro data is not available
|
|
// for clarification of the sign see definition of flowSample and imuSample in common.h
|
|
if ((_delta_time_of > FLT_EPSILON)
|
|
&& (_flow_sample_delayed.dt > FLT_EPSILON)) {
|
|
|
|
_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt;
|
|
_flow_gyro_bias.zero();
|
|
|
|
is_body_rate_comp_available = true;
|
|
}
|
|
}
|
|
|
|
// reset the accumulators
|
|
_imu_del_ang_of.setZero();
|
|
_delta_time_of = 0.0f;
|
|
return is_body_rate_comp_available;
|
|
}
|
|
|
|
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
|
|
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
|
|
{
|
|
// calculate the observation noise variance - scaling noise linearly across flow quality range
|
|
const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
|
|
const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
|
|
|
|
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
|
|
float weighting = (255.f - (float)_params.flow_qual_min);
|
|
|
|
if (weighting >= 1.f) {
|
|
weighting = math::constrain((float)(flow_sample.quality - _params.flow_qual_min) / weighting, 0.f, 1.f);
|
|
|
|
} else {
|
|
weighting = 0.0f;
|
|
}
|
|
|
|
// take the weighted average of the observation noise for the best and wort flow quality
|
|
const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.f - weighting));
|
|
|
|
return R_LOS;
|
|
}
|