fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 10:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #265 from PX4/pr-ekfOutputObserver

Browse Source 
EKF: add option to output velocity as time derivative of position
This commit is contained in:
Paul Riseborough
2017-06-12 19:01:54 +10:00
committed by GitHub
parent a17879ab91 fed4a9bc5a
commit 97eb17a62f
5 changed files with 155 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/RingBuffer.h
+5
View File
@@ -113,6 +113,11 @@ public:
return _buffer[_tail];
}
unsigned get_oldest_index()
{
return _tail;
}
inline data_type get_newest()
{
return _buffer[_head];
EKF/common.h
+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 {
EKF/ekf.cpp
+119 -27
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();
}
}
}
EKF/estimator_interface.cpp
+3 -1
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();
}
EKF/estimator_interface.h
+18 -14
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