From 5779ebd1776db57754eeb5d9035a00069586a412 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Aug 2021 21:55:02 +0900 Subject: [PATCH] AP_NavEKF3: remove unused Tbn_flow --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 -- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 - 2 files changed, 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 17dbc15d78..41bcb31354 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -193,8 +193,6 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f // need to run the optical flow takeoff detection detectOptFlowTakeoff(); - // calculate rotation matrices at mid sample time for flow observations - stateStruct.quat.rotation_matrix(Tbn_flow); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { // correct flow sensor body rates for bias and write diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b0c6da6aec..673d804840 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1170,7 +1170,6 @@ private: uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec) uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec) - Matrix3F Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2 Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) ftype Popt; // Optical flow terrain height state covariance (m^2)