EKF: add vertical position derivative output

Add calculation of a vertical position derivative to the output predictor. This will have degraded tracking relative to the EKF states, but the velocity will be closer to the first derivative of the position and reduce the effect inertial prediction errors on control loops that are operating in a pure velocity feedback mode.
Move calculation of IMU offset angular rate correction out of velocity accessor and into output predictor.
Provide separate accessor for vertical position derivative.
This commit is contained in:
Paul Riseborough
2017-06-12 10:45:46 +10:00
parent a17879ab91
commit fed4a9bc5a
5 changed files with 155 additions and 45 deletions
+10 -3
View File
@@ -79,9 +79,16 @@ struct ext_vision_message {
struct outputSample {
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
Vector3f vel; // NED velocity estimate in earth frame in m/s
Vector3f pos; // NED position estimate in earth frame in m/s
uint64_t time_us; // timestamp in microseconds
Vector3f vel; // NED velocity estimate in earth frame in m/s
Vector3f pos; // NED position estimate in earth frame in m/s
uint64_t time_us; // timestamp in microseconds
};
struct outputVert {
float vel_d; // D velocity calculated using alternative algorithm
float vel_d_integ; // Integral of vel_d
float dt; // delta time in seconds
uint64_t time_us; // timestamp in microseconds
};
struct imuSample {