Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: small performance improvement by replacing mul_transpose with direct multiplication of Z axis accel correction to specific elements of dcm

This commit is contained in:
rmackay9 2012-12-12 23:30:52 +09:00
parent ad14e727e2
commit 59b20c6fe4
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -343,6 +343,7 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
{
static uint8_t first_reads = 0;
float hist_position_base_z;
float accel_ef_z_correction;
// discard samples where dt is too large
if( dt > 0.2 ) {
@ -369,9 +370,16 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
// calculate error in position from baro with our estimate
float err = baro_alt - (hist_position_base_z + _position_correction.z);
// calculate correction to accelerometers and apply in the body frame
// retrieve the existing accelerometer corrections
Vector3f accel_corr = accel_correction.get();
accel_corr += dcm.mul_transpose(Vector3f(0,0,(err*_k3_z) * dt));
// calculate the accelerometer correction from this iteration in the earth frame
accel_ef_z_correction = err * _k3_z * dt;
// rotate the correction into the body frame (note: this is a shortened form of dcm.mul_transpose(..) because we have only one axis
accel_corr.x += accel_ef_z_correction * dcm.c.x;
accel_corr.y += accel_ef_z_correction * dcm.c.y;
accel_corr.z += accel_ef_z_correction * dcm.c.z;
accel_correction.set(accel_corr);
// correct velocity