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

AP_NavEKF2: Remove unused code 

This method of correcting for the fusion time horizon delay was too computationally expensive for our application and did not smooth fusion noise.
This commit is contained in:
Paul Riseborough 2015-11-06 11:56:39 +11:00 committed by Andrew Tridgell
parent d48454ee2d
commit 3eeff8dbc6
2 changed files with 0 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -482,72 +482,6 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
ConstrainStates();
}
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using buffered IMU data
void NavEKF2_core::calcOutputStates() {
// initialise the store access at the fusion time horizon (it will be advanced later)
uint8_t imuStoreAccessIndex = fifoIndexDelayed;
imu_elements imuData;
// Counter used to ensure the while loop always exits
uint8_t watchdog = 0;
// initialise to the solution at the fusion time horizon
outputDataNew.quat = stateStruct.quat;
outputDataNew.velocity = stateStruct.velocity;
outputDataNew.position = stateStruct.position;
// Iterate through the buffered IMU data, using the strapdown equations to wind forward from the fusion time horizon to current time
// we stop iterating when we have reached the current imu Data
do {
// If the loop cannot exit, force exit
if (watchdog > IMU_BUFFER_LENGTH+2) {
return;
}
watchdog++;
// advance to the next index
imuStoreAccessIndex++;
// if we have got to the end of the array, return to the start
if (imuStoreAccessIndex >= IMU_BUFFER_LENGTH) {
imuStoreAccessIndex = 0;
}
imuData = storedIMU[imuStoreAccessIndex];
// remove gyro bias errors
Vector3f delAng = imuData.delAng - stateStruct.gyro_bias;
// remove Z accel bias error
Vector3f delVel = imuData.delVel;
delVel.z -= stateStruct.accel_zbias;
// convert the rotation vector to its equivalent quaternion
Quaternion deltaQuat;
deltaQuat.from_axis_angle_fast(delAng);
// update the quaternion states by rotating from the previous attitude through
// the delta angle rotation quaternion and normalise
outputDataNew.quat *= deltaQuat;
outputDataNew.quat.normalize();
// calculate the body to nav cosine matrix
Matrix3f Tbn_temp;
outputDataNew.quat.rotation_matrix(Tbn_temp);
// transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded
Vector3f delVelNav = Tbn_temp*delVel;
delVelNav.z += GRAVITY_MSS*imuData.delVelDT;
// use a simple Euler integration
outputDataNew.position += outputDataNew.velocity*imuData.delVelDT;
}
while (imuStoreAccessIndex != fifoIndexNow);
}
/*
* Propagate PVA solution forward from the fusion time horizon to the current time horizon
* using simple observer which performs two functions:

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -614,10 +614,6 @@ private:
// Calculate compass heading innovation
float calcMagHeadingInnov();
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using buffered IMU data
void calcOutputStates();
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
void calcOutputStatesFast();