fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 22:27:36 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+2 -3
View File
@@ -65,9 +65,6 @@ void Ekf::fuseOptFlow()
// calculate the optical flow observation variance // calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(); 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 // get rotation matrix from earth to body
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
@@ -262,6 +259,8 @@ void Ekf::fuseOptFlow()
} }
// fuse observation axes sequentially // 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++) { for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// calculate observation Jocobians and Kalman gains // calculate observation Jocobians and Kalman gains