fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:44:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: add vertical position derivative output

Browse Source 
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-04-23 09:04:00 +10:00
parent a17879ab91
commit fed4a9bc5a
5 changed files with 155 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/RingBuffer.h
View File

@ -113,6 +113,11 @@ public:
return _buffer[_tail];
}
unsigned get_oldest_index()
{
return _tail;
}
inline data_type get_newest()
{
return _buffer[_head];

13
EKF/common.h
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 {

146
EKF/ekf.cpp
View File

@ -478,10 +478,10 @@ bool Ekf::collect_imu(imuSample &imu)
*/
void Ekf::calculateOutputStates()
{
// use latest IMU data
// Use full rate IMU data at the current time horizon
imuSample imu_new = _imu_sample_new;
// correct delta angles for bias offsets and scale factors
// correct delta angles for bias offsets
Vector3f delta_angle;
float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction;
@ -528,19 +528,38 @@ void Ekf::calculateOutputStates()
Vector3f vel_last = _output_new.vel;
// increment the INS velocity states by the measurement plus corrections
// do the same for vertical state used by alternative correction algorithm
_output_new.vel += delta_vel_NED;
_output_vert_new.vel_d += delta_vel_NED(2);
// use trapezoidal integration to calculate the INS position states
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f);
// do the same for vertical state used by alternative correction algorithm
Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f);
_output_new.pos += delta_pos_NED;
_output_vert_new.vel_d_integ += delta_pos_NED(2);
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
// accumulate the time for each update
_output_vert_new.dt += imu_new.delta_vel_dt;
// calculate the average angular rate across the last IMU update
Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
// calculate the velocity of the IMU relative to the body origin
Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
// rotate the relative velocity into earth frame
_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
if (_imu_updated) {
_output_buffer.push(_output_new);
_output_vert_buffer.push(_output_vert_new);
_imu_updated = false;
// get the oldest INS state data from the ring buffer
// this data will be at the EKF fusion time horizon
_output_sample_delayed = _output_buffer.get_oldest();
_output_vert_delayed = _output_vert_buffer.get_oldest();
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed();
@ -581,37 +600,110 @@ void Ekf::calculateOutputStates()
_output_tracking_error[1] = vel_err.norm();
_output_tracking_error[2] = pos_err.norm();
// calculate a velocity correction that will be applied to the output state history
/*
* Loop through the output filter state history and apply the corrections to the velocity and position states.
* This method is too expensive to use for the attitude states due to the quaternion operations required
* but becasue it eliminates the time delay in the 'correction loop' it allows higher tracking gains
* to be used and reduces tracking error relative to EKF states.
*/
// Complementary filter gains
float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
_vel_err_integ += vel_err;
Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
// calculate a position correction that will be applied to the output state history
float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
_pos_err_integ += pos_err;
Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
{
/*
* Calculate a correction to be applied to vel_d that casues vel_d_integ to track the EKF
* down position state at the fusion time horizon using an alternative algorithm to what
* is used for the vel and pos state tracking. The algorithm applies a correction to the vel_d
* state history and propagates vel_d_integ forward in time using the corrected vel_d history.
* This provides an alternative vertical velocity output that is closer to the first derivative
* of the position but does degrade tracking relative to the EKF state.
*/
// loop through the output filter state history and apply the corrections to the velocity and position states
// this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
// to be used
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
// calculate down velocity and position tracking errors
float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
// calculate a velocity correction that will be applied to the output state history
// using a PD feedback tuned to a 5% overshoot
float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
// a constant velocity correction is applied
output_states.vel += vel_correction;
/*
* Calculate corrections to be applied to vel and pos output state history.
* The vel and pos state history are corrected individually so they track the EKF states at
* the fusion time horizon. This option provides the most accurate tracking of EKF states.
*/
// a constant position correction is applied
output_states.pos += pos_correction;
// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
// vel_d states and propagate vel_d_integ forward using the corrected vel_d
outputVert current_state;
outputVert next_state;
unsigned index = _output_vert_buffer.get_oldest_index();
unsigned index_next;
unsigned size = _output_vert_buffer.get_length();
for (unsigned counter=0; counter < (size - 1); counter++) {
index_next = (index + 1) % size;
current_state = _output_vert_buffer.get_from_index(index);
next_state = _output_vert_buffer.get_from_index(index_next);
// push the updated data to the buffer
_output_buffer.push_to_index(index, output_states);
// correct the velocity
if (counter == 0) {
current_state.vel_d += vel_d_correction;
_output_vert_buffer.push_to_index(index,current_state);
}
next_state.vel_d += vel_d_correction;
// position is propagated forward using the corrected velocity and a trapezoidal integrator
next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
// push the updated data to the buffer
_output_vert_buffer.push_to_index(index_next,next_state);
// advance the index
index = (index + 1) % size;
}
// update output state to corrected values
_output_vert_new = _output_vert_buffer.get_newest();
// reset time delta to zero for the next accumulation of full rate IMU data
_output_vert_new.dt = 0.0f;
}
// update output state to corrected values
_output_new = _output_buffer.get_newest();
{
/*
* Calculate corrections to be applied to vel and pos output state history.
* The vel and pos state history are corrected individually so they track the EKF states at
* the fusion time horizon. This option provides the most accurate tracking of EKF states.
*/
// calculate a velocity correction that will be applied to the output state history
_vel_err_integ += vel_err;
Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
// calculate a position correction that will be applied to the output state history
_pos_err_integ += pos_err;
Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
// loop through the output filter state history and apply the corrections to the velocity and position states
outputSample output_states;
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
// a constant velocity correction is applied
output_states.vel += vel_correction;
// a constant position correction is applied
output_states.pos += pos_correction;
// push the updated data to the buffer
_output_buffer.push_to_index(index,output_states);
}
// update output state to corrected values
_output_new = _output_buffer.get_newest();
}
}
}

4
EKF/estimator_interface.cpp
View File

@ -379,7 +379,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_flow_buffer.allocate(_obs_buffer_length) &&
_ext_vision_buffer.allocate(_obs_buffer_length) &&
_drag_buffer.allocate(_obs_buffer_length) &&
_output_buffer.allocate(_imu_buffer_length))) {
_output_buffer.allocate(_imu_buffer_length) &&
_output_vert_buffer.allocate(_imu_buffer_length))) {
ECL_ERR("EKF buffer allocation failed!");
unallocate_buffers();
return false;
@ -448,6 +449,7 @@ void EstimatorInterface::unallocate_buffers()
_flow_buffer.unallocate();
_ext_vision_buffer.unallocate();
_output_buffer.unallocate();
_output_vert_buffer.unallocate();
}

32
EKF/estimator_interface.h
View File

@ -200,30 +200,29 @@ public:
// return true if the local position estimate is valid
bool local_position_is_valid();
void copy_quaternion(float *quat)
{
for (unsigned i = 0; i < 4; i++) {
quat[i] = _output_new.quat_nominal(i);
}
}
// get the velocity of the body frame origin in local NED earth frame
void get_velocity(float *vel)
{
// calculate the average angular rate across the last IMU update
Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
// calculate the velocity of the relative to the body origin
// Note % operator has been overloaded to performa cross product
Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
// rotate the relative velocty into earth frame and subtract from the EKF velocity
// (which is at the IMU) to get velocity of the body origin
Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body;
// copy to output
Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
for (unsigned i = 0; i < 3; i++) {
vel[i] = vel_earth(i);
}
}
// get the derivative of the vertical position of the body frame origin in local NED earth frame
void get_pos_d_deriv(float *pos_d_deriv)
{
float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
*pos_d_deriv = var;
}
// get the position of the body frame origin in local NED earth frame
void get_position(float *pos)
{
@ -333,10 +332,14 @@ protected:
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
float _air_density{1.225f}; // air density (kg/m**3)
// Output Predictor
outputSample _output_sample_delayed{}; // filter output on the delayed time horizon
outputSample _output_new{}; // filter output on the non-delayed time horizon
imuSample _imu_sample_new{}; // imu sample capturing the newest imu data
Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
outputSample _output_new{}; // filter output on the non-delayed time horizon
outputVert _output_vert_delayed{}; // vertical filter output on the delayed time horizon
outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon
imuSample _imu_sample_new{}; // imu sample capturing the newest imu data
Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame
uint64_t _imu_ticks{0}; // counter for imu updates
@ -381,6 +384,7 @@ protected:
RingBuffer<flowSample> _flow_buffer;
RingBuffer<extVisionSample> _ext_vision_buffer;
RingBuffer<outputSample> _output_buffer;
RingBuffer<outputVert> _output_vert_buffer;
RingBuffer<dragSample> _drag_buffer;
uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds