From bd8c804ab5c5363b540a0429e247fc0c39205e6f Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 31 May 2016 16:05:59 -0700 Subject: [PATCH] AP_NavEKF2: correct comments --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 6bbc0be68c..fe9bf1f8a3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -269,10 +269,11 @@ void NavEKF2_core::readIMUData() imuQuatDownSampleNew.rotate(imuDataNew.delAng); imuQuatDownSampleNew.normalize(); - // Rotate the latest delta velocity into the frame of reference at the start of - // accumulate the latest delta velocity and apply it to the delta velocity accumulator + // Rotate the latest delta velocity into body frame at the start of accumulation Matrix3f deltaRotMat; imuQuatDownSampleNew.rotation_matrix(deltaRotMat); + + // Apply the delta velocity to the delta velocity accumulator imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel; // Keep track of the number of IMU frames since the last state prediction