aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialNav: Jason's bug fix to inertial nav velocity and position calculations

Browse Source
This commit is contained in:
rmackay9 2012-12-18 16:38:56 +09:00 committed by Andrew Tridgell
parent d3cbf733ba
commit 37b56662bd
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -51,6 +51,7 @@ void AP_InertialNav::update(float dt)
{ {
Vector3f acc_corr = accel_correction.get(); Vector3f acc_corr = accel_correction.get();
Vector3f accel_ef; Vector3f accel_ef;
Vector3f velocity_increase;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 0.1 ) { if( dt > 0.1 ) {
@ -80,11 +81,14 @@ void AP_InertialNav::update(float dt)
// get earth frame accelerometer correction // get earth frame accelerometer correction
accel_correction_ef = dcm * acc_corr; accel_correction_ef = dcm * acc_corr;
// calculate velocity by adding new acceleration from accelerometers // calculate velocity increase adding new acceleration from accelerometers
_velocity += (-accel_ef + accel_correction_ef) * dt; velocity_increase = (-accel_ef + accel_correction_ef) * dt;
// calculate new estimate of position // calculate new estimate of position
_position_base += _velocity * dt; _position_base += (_velocity + velocity_increase*0.5) * dt;
// calculate new velocity
_velocity += velocity_increase;
// store 3rd order estimate (i.e. estimated vertical position) for future use // store 3rd order estimate (i.e. estimated vertical position) for future use
_hist_position_estimate_z.add(_position_base.z); _hist_position_estimate_z.add(_position_base.z);