dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

implemented prediction of states and covariance matrix

This commit is contained in:
Roman 2015-12-06 12:57:43 +01:00
parent 16eb3b0e96
commit cfc39bc2f9
4 changed files with 144 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
EKF/ekf.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,13 +33,14 @@
/**
* @file ekf.cpp
* Definition of ekf attitude position estimator class.
* Core functions for ekf attitude and position estimator.
*
* @author Roman Bast <bastroman@gmail.com>
* @author Roman Bast <bapstroman@gmail.com>
*
*/
#include "ekf.h"
#include <drivers/drv_hrt.h>
Ekf::Ekf()
{
@ -58,10 +59,6 @@ void Ekf::update()
_filter_initialised = initialiseFilter();
}
if (!_filter_initialised) {
return;
}
// prediction
if (_imu_updated) {
predictState();
@ -70,15 +67,15 @@ void Ekf::update()
}
// measurement updates
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _mag_sample_delayed)) {
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
fuseMag();
}
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _baro_sample_delayed)) {
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
_fuse_height = true;
}
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _gps_sample_delayed)) {
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
_fuse_pos = true;
_fuse_vel = true;
}
@ -87,12 +84,94 @@ void Ekf::update()
fusePosVel();
}
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sample_delayed)) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
fuseRange();
}
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _airspeed_sample_delayed)) {
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
fuseAirspeed();
}
}
}
bool Ekf::initialiseFilter(void)
{
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial attitude estimate from accel vector, assuming vehicle is static
Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
float pitch = 0.0f;
float roll = 0.0f;
if (accel_init.norm() > 0.001f) {
accel_init.normalize();
pitch = asinf(accel_init(0));
roll = -asinf(accel_init(1) / cosf(pitch));
}
matrix::Euler<float> euler_init(0, pitch, roll);
_state.quat_nominal = Quaternion(euler_init);
resetVelocity();
resetPosition();
initialiseCovariance();
return true;
}
void Ekf::predictState()
{
// compute transformation matrix from body to world frame
matrix::Dcm<float> R(_state.quat_nominal);
R.transpose();
// attitude error state prediciton
Quaternion dq;
dq.from_axis_angle(_imu_sample_delayed.delta_ang);
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize();
Vector3f vel_last = _state.vel;
// predict velocity states
_state.vel += R * _imu_sample_delayed.delta_vel;
_state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt;
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
//matrix::Euler<float> euler(_state.quat_nominal);
//printf("roll pitch yaw %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
}
void Ekf::fusePosVel()
{
}
void Ekf::fuseMag()
{
}
void Ekf::fuseAirspeed()
{
}
void Ekf::fuseRange()
{
}

17
EKF/ekf.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,9 +33,9 @@
/**
* @file ekf.h
* Definition of ekf attitude position estimator class.
* Class for core functions for ekf attitude and position estimator.
*
* @author Roman Bast <bastroman@gmail.com>
* @author Roman Bast <bapstroman@gmail.com>
*
*/
@ -52,14 +52,19 @@ public:
private:
static const uint8_t _kNumStates = 24;
bool _filter_initialised;
bool _fuse_height; // true if there is new baro data
bool _fuse_pos; // true if there is new position data from gps
bool _fuse_vel; // true if there is new velocity data from gps
float P[_kNumStates][_kNumStates]; // state covariance matrix
bool initialiseFilter(void);
void initialiseCovariance();
void predictState();
void predictCovariance();
@ -72,5 +77,9 @@ private:
void fuseRange();
void resetVelocity();
};
void resetPosition();
};

19
EKF/estimator_base.cpp
View File

@ -35,7 +35,7 @@
* @file estimator_base.cpp
* Definition of base class for attitude estimators
*
* @author Roman Bast <bastroman@gmail.com>
* @author Roman Bast <bapstroman@gmail.com>
*
*/
@ -264,6 +264,21 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
_params.airspeed_delay_ms = 0;
_params.requiredEph = 10;
_params.requiredEpv = 10;
_params.dax_noise = 0.0f;
_params.day_noise = 0.0f;
_params.daz_noise = 0.0f;
_params.dvx_noise = 0.0f;
_params.dvy_noise = 0.0f;
_params.dvz_noise = 0.0f;
_params.delta_ang_sig = 0.0f;
_params.delta_vel_sig = 0.0f;
_params.delta_pos_sig = 0.0f;
_params.delta_gyro_bias_sig = 0.0f;
_params.delta_gyro_scale_sig = 0.0f;
_params.delta_vel_bias_z_sig = 0.0f;
_params.delta_mag_body_sig = 0.0f;
_params.delta_mag_earth_sig = 0.0f;
_params.delta_wind_sig = 0.0f;
_dt_imu_avg = 0.0f;
_imu_time_last = time_usec;
@ -400,4 +415,4 @@ void EstimatorBase::printStoredGps()
for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
printGps(&_gps_buffer[i]);
}
}
}

26
EKF/estimator_base.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,7 +35,7 @@
* @file estimator_base.h
* Definition of base class for attitude estimators
*
* @author Roman Bast <bastroman@gmail.com>
* @author Roman Bast <bapstroman@gmail.com>
*
*/
@ -64,6 +64,8 @@ public:
EstimatorBase();
~EstimatorBase();
virtual void update() = 0;
// set delta angle imu data
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
@ -159,8 +161,25 @@ protected:
uint32_t airspeed_delay_ms;
float requiredEph;
float requiredEpv;
} _params;
float dax_noise;
float day_noise;
float daz_noise;
float dvx_noise;
float dvy_noise;
float dvz_noise;
float delta_ang_sig;
float delta_vel_sig;
float delta_pos_sig;
float delta_gyro_bias_sig;
float delta_gyro_scale_sig;
float delta_vel_bias_z_sig;
float delta_mag_body_sig;
float delta_mag_earth_sig;
float delta_wind_sig;
} _params;
static const uint8_t OBS_BUFFER_LENGTH = 5;
static const uint8_t IMU_BUFFER_LENGTH = 25;
@ -228,4 +247,3 @@ public:
void printGps(struct gpsSample *data);
void printStoredGps();
};