From 5907e81ef7e38d3104f5d1ef9f2a56c365bccd58 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 15 Aug 2020 08:49:42 +1000 Subject: [PATCH] EKF: optflow fusion cleanup --- EKF/optflow_fusion.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 8ea92687af..0834deb6cd 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -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