forked from Archive/PX4-Autopilot
added complementary filter for real time state estimation
This commit is contained in:
Roman Bapst
parent
67646a15b0
commit
d233ca3990
119
EKF/ekf.cpp
119
EKF/ekf.cpp
|
|
@ -52,6 +52,9 @@ Ekf::Ekf():
|
|||
{
|
||||
_earth_rate_NED.setZero();
|
||||
_R_prev = matrix::Dcm<float>();
|
||||
_delta_angle_corr.setZero();
|
||||
_delta_vel_corr.setZero();
|
||||
_vel_corr.setZero();
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -65,8 +68,12 @@ bool Ekf::update()
|
|||
bool ret = false; // indicates if there has been an update
|
||||
if (!_filter_initialised) {
|
||||
_filter_initialised = initialiseFilter();
|
||||
if (!_filter_initialised) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
printStates();
|
||||
//printStatesFast();
|
||||
// prediction
|
||||
if (_imu_updated) {
|
||||
ret = true;
|
||||
|
|
@ -105,14 +112,7 @@ bool Ekf::update()
|
|||
fuseAirspeed();
|
||||
}
|
||||
|
||||
// write to output if this has been a prediction step
|
||||
if (_imu_updated) {
|
||||
_output_delayed.vel = _state.vel;
|
||||
_output_delayed.pos = _state.pos;
|
||||
_output_delayed.quat_nominal = _state.quat_nominal;
|
||||
_output_delayed.time_us = _imu_time_last;
|
||||
_imu_updated = false;
|
||||
}
|
||||
calculateOutputStates();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -142,9 +142,18 @@ bool Ekf::initialiseFilter(void)
|
|||
roll = -asinf(accel_init(1) / cosf(pitch));
|
||||
}
|
||||
|
||||
matrix::Euler<float> euler_init(0, pitch, roll);
|
||||
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
|
||||
magSample mag_init = _mag_buffer.get_newest();
|
||||
if (mag_init.time_us == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
matrix::Dcm<float> R_to_earth(euler_init);
|
||||
_state.mag_I = R_to_earth * mag_init.mag;
|
||||
|
||||
|
||||
resetVelocity();
|
||||
resetPosition();
|
||||
|
||||
|
|
@ -184,6 +193,73 @@ void Ekf::predictState()
|
|||
constrainStates();
|
||||
}
|
||||
|
||||
void Ekf::calculateOutputStates()
|
||||
{
|
||||
imuSample imu_new = _imu_sample_new;
|
||||
Vector3f delta_angle;
|
||||
delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0);
|
||||
delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1);
|
||||
delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2);
|
||||
|
||||
delta_angle -= _state.gyro_bias;
|
||||
|
||||
Vector3f delta_vel = imu_new.delta_vel;
|
||||
delta_vel(2) -= _state.accel_z_bias;
|
||||
|
||||
delta_angle += _delta_angle_corr;
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(delta_angle);
|
||||
|
||||
_output_new.time_us = imu_new.time_us;
|
||||
_output_new.quat_nominal = dq * _output_new.quat_nominal;
|
||||
_output_new.quat_nominal.normalize();
|
||||
|
||||
matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
|
||||
|
||||
Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
|
||||
delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
|
||||
|
||||
Vector3f vel_last = _output_new.vel;
|
||||
|
||||
_output_new.vel += delta_vel_NED;
|
||||
|
||||
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;
|
||||
|
||||
if (_imu_updated) {
|
||||
_output_buffer.push(_output_new);
|
||||
_imu_updated = false;
|
||||
}
|
||||
|
||||
if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Quaternion quat_inv = _state.quat_nominal.inversed();
|
||||
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
|
||||
q_error.normalize();
|
||||
Vector3f delta_ang_error;
|
||||
|
||||
float scalar;
|
||||
|
||||
if (q_error(0) >= 0.0f) {
|
||||
scalar = -2.0f;
|
||||
} else {
|
||||
scalar = 2.0f;
|
||||
}
|
||||
|
||||
delta_ang_error(0) = scalar * q_error(1);
|
||||
delta_ang_error(1) = scalar * q_error(2);
|
||||
delta_ang_error(2) = scalar * q_error(3);
|
||||
|
||||
_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;
|
||||
|
||||
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
|
||||
|
||||
_vel_corr = (_state.pos - _output_sample_delayed.pos);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ekf::fuseAirspeed()
|
||||
{
|
||||
|
|
@ -237,3 +313,28 @@ void Ekf::printStates()
|
|||
counter++;
|
||||
|
||||
}
|
||||
|
||||
void Ekf::printStatesFast()
|
||||
{
|
||||
static int counter_fast = 0;
|
||||
|
||||
if (counter_fast % 50 == 0) {
|
||||
printf("quaternion\n");
|
||||
for(int i = 0; i < 4; i++) {
|
||||
printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
|
||||
}
|
||||
|
||||
printf("vel\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("v %i %.5f\n", i, (double)_output_new.vel(i));
|
||||
}
|
||||
|
||||
printf("pos\n");
|
||||
for(int i = 0; i < 3; i++) {
|
||||
printf("p %i %.5f\n", i, (double)_output_new.pos(i));
|
||||
}
|
||||
|
||||
counter_fast = 0;
|
||||
}
|
||||
counter_fast++;
|
||||
}
|
||||
|
|
|
|||
10
EKF/ekf.h
10
EKF/ekf.h
|
|
@ -72,6 +72,13 @@ private:
|
|||
|
||||
float P[_k_num_states][_k_num_states]; // state covariance matrix
|
||||
|
||||
// complementary filter states
|
||||
Vector3f _delta_angle_corr;
|
||||
Vector3f _delta_vel_corr;
|
||||
Vector3f _vel_corr;
|
||||
|
||||
void calculateOutputStates();
|
||||
|
||||
bool initialiseFilter(void);
|
||||
|
||||
void initialiseCovariance();
|
||||
|
|
@ -110,7 +117,8 @@ private:
|
|||
|
||||
void printStates();
|
||||
|
||||
void printStatesFast();
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
|
||||
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -83,6 +83,8 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
|
|||
|
||||
imu_sample_new.time_us = time_usec;
|
||||
|
||||
_imu_sample_new = imu_sample_new;
|
||||
|
||||
imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
|
||||
imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
|
||||
imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
|
||||
|
|
@ -273,7 +275,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
|
|||
_params.gps_pos_noise = 1.0f;
|
||||
_params.baro_noise = 0.1f;
|
||||
_params.mag_heading_noise = 3e-2f;
|
||||
_params.mag_declination_deg = 10.0f;
|
||||
_params.mag_declination_deg = 0.0f;
|
||||
_params.heading_innov_gate = 0.5f;
|
||||
|
||||
_dt_imu_avg = 0.0f;
|
||||
|
|
@ -285,6 +287,10 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
|
|||
_imu_sample_delayed.delta_vel_dt = 0.0f;
|
||||
_imu_sample_delayed.time_us = time_usec;
|
||||
|
||||
_output_new.vel.setZero();
|
||||
_output_new.pos.setZero();
|
||||
_output_new.quat_nominal = matrix::Quaternion<float>();
|
||||
|
||||
_imu_down_sampled.delta_ang.setZero();
|
||||
_imu_down_sampled.delta_vel.setZero();
|
||||
_imu_down_sampled.delta_ang_dt = 0.0f;
|
||||
|
|
|
|||
|
|
@ -202,7 +202,9 @@ protected:
|
|||
airspeedSample _airspeed_sample_delayed;
|
||||
flowSample _flow_sample_delayed;
|
||||
|
||||
outputSample _output_delayed;
|
||||
outputSample _output_sample_delayed;
|
||||
outputSample _output_new;
|
||||
imuSample _imu_sample_new;
|
||||
|
||||
struct map_projection_reference_s _posRef;
|
||||
float _gps_alt_ref;
|
||||
|
|
@ -266,17 +268,17 @@ public:
|
|||
|
||||
void copy_quaternion(float *quat) {
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
quat[i] = _state.quat_nominal(i);
|
||||
quat[i] = _output_new.quat_nominal(i);
|
||||
}
|
||||
}
|
||||
void copy_velocity(float *vel) {
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
vel[i] = _state.vel(i);
|
||||
vel[i] = _output_new.vel(i);
|
||||
}
|
||||
}
|
||||
void copy_position(float *pos) {
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
pos[i] = _state.pos(i);
|
||||
pos[i] = _output_new.pos(i);
|
||||
}
|
||||
}
|
||||
void copy_timestamp(uint64_t *time_us) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue