Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: correct comments

Browse Source
This commit is contained in:
Jonathan Challinger 2016-05-31 16:05:59 -07:00 committed by Andrew Tridgell
parent ebae95d7f6
commit bd8c804ab5
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -269,10 +269,11 @@ void NavEKF2_core::readIMUData()
imuQuatDownSampleNew.rotate(imuDataNew.delAng); imuQuatDownSampleNew.rotate(imuDataNew.delAng);
imuQuatDownSampleNew.normalize(); imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into the frame of reference at the start of // Rotate the latest delta velocity into body frame at the start of accumulation
// accumulate the latest delta velocity and apply it to the delta velocity accumulator
Matrix3f deltaRotMat; Matrix3f deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat); imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel; imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
// Keep track of the number of IMU frames since the last state prediction // Keep track of the number of IMU frames since the last state prediction