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

EKF: optflow fusion cleanup

Browse Source
This commit is contained in:
Paul Riseborough 2020-08-15 08:49:42 +10:00 committed by Paul Riseborough
parent 7fa9a1963b
commit 5907e81ef7
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/optflow_fusion.cpp
View File

@ -65,9 +65,6 @@ void Ekf::fuseOptFlow()
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar();
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector24f Kfusion; // Optical flow Kalman gains
// get rotation matrix from earth to body
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
@ -262,6 +259,8 @@ void Ekf::fuseOptFlow()
}
// fuse observation axes sequentially
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector24f Kfusion; // Optical flow Kalman gains
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// calculate observation Jocobians and Kalman gains