mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 17:59:06 +08:00
-added comments
-removed unused print functions -removed false _imu_time_last variable (correct is _time_last_imu)
This commit is contained in:
Roman
parent
9d7340e187
commit
ce0ddc0207
@ -82,13 +82,8 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
inline void push(data_type sample, bool debug = false)
|
||||
inline void push(data_type sample)
|
||||
{
|
||||
if (debug) {
|
||||
printf("elapsed %" PRIu64 "\n", sample.time_us - _time_last);
|
||||
_time_last = sample.time_us;
|
||||
}
|
||||
|
||||
int head_new = _head;
|
||||
|
||||
if (_first_write) {
|
||||
|
||||
62
EKF/common.h
62
EKF/common.h
@ -64,45 +64,45 @@ typedef matrix::Quaternion<float> Quaternion;
|
||||
typedef matrix::Matrix<float, 3, 3> Matrix3f;
|
||||
|
||||
struct outputSample {
|
||||
Quaternion quat_nominal;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
uint64_t time_us;
|
||||
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
|
||||
};
|
||||
|
||||
struct imuSample {
|
||||
Vector3f delta_ang;
|
||||
Vector3f delta_vel;
|
||||
float delta_ang_dt;
|
||||
float delta_vel_dt;
|
||||
uint64_t time_us;
|
||||
Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements)
|
||||
Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements)
|
||||
float delta_ang_dt; // delta angle integration period in seconds
|
||||
float delta_vel_dt; // delta velocity integration period in seconds
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct gpsSample {
|
||||
Vector2f pos;
|
||||
float hgt;
|
||||
Vector3f vel;
|
||||
uint64_t time_us;
|
||||
Vector2f pos; // NE earth frame gps horizontal position measurement in m
|
||||
float hgt; // gps height measurement in m
|
||||
Vector3f vel; // NED earth frame gps velocity measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct magSample {
|
||||
Vector3f mag;
|
||||
uint64_t time_us;
|
||||
Vector3f mag; // NED magnetometer body frame measurements
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct baroSample {
|
||||
float hgt;
|
||||
uint64_t time_us;
|
||||
float hgt; // barometer height above sea level measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct rangeSample {
|
||||
float rng;
|
||||
uint64_t time_us;
|
||||
float rng; // range (distance to ground) measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct airspeedSample {
|
||||
float airspeed;
|
||||
uint64_t time_us;
|
||||
float airspeed; // airspeed measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct flowSample {
|
||||
@ -155,16 +155,16 @@ struct parameters {
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
Vector3f ang_error;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
Vector3f gyro_bias;
|
||||
Vector3f gyro_scale;
|
||||
float accel_z_bias;
|
||||
Vector3f mag_I;
|
||||
Vector3f mag_B;
|
||||
Vector2f wind_vel;
|
||||
Quaternion quat_nominal;
|
||||
Vector3f ang_error; // attitude axis angle error (error state formulation)
|
||||
Vector3f vel; // NED velocity in earth frame in m/s
|
||||
Vector3f pos; // NED position in earth frame in m
|
||||
Vector3f gyro_bias; // gyro bias estimate in rad/s
|
||||
Vector3f gyro_scale; // gyro scale estimate
|
||||
float accel_z_bias; // accelerometer z axis bias estimate
|
||||
Vector3f mag_I; // NED earth magnetic field in gauss
|
||||
Vector3f mag_B; // magnetometer bias estimate in body frame in gauss
|
||||
Vector2f wind_vel; // wind velocity in m/s
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
};
|
||||
|
||||
struct fault_status_t {
|
||||
|
||||
81
EKF/ekf.cpp
81
EKF/ekf.cpp
@ -40,7 +40,6 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
Ekf::Ekf():
|
||||
_control_status{},
|
||||
@ -443,83 +442,3 @@ void Ekf::fuseRange()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void Ekf::printStates()
|
||||
{
|
||||
static int counter = 0;
|
||||
|
||||
if (counter % 50 == 0) {
|
||||
printf("quaternion\n");
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
|
||||
}
|
||||
|
||||
matrix::Euler<float> euler(_state.quat_nominal);
|
||||
printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
|
||||
|
||||
printf("vel\n");
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
printf("v %i %.5f\n", i, (double)_state.vel(i));
|
||||
}
|
||||
|
||||
printf("pos\n");
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
printf("p %i %.5f\n", i, (double)_state.pos(i));
|
||||
}
|
||||
|
||||
printf("gyro_scale\n");
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
|
||||
}
|
||||
|
||||
printf("mag earth\n");
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
|
||||
}
|
||||
|
||||
printf("mag bias\n");
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
|
||||
}
|
||||
|
||||
counter = 0;
|
||||
}
|
||||
|
||||
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++;
|
||||
}
|
||||
|
||||
50
EKF/ekf.h
50
EKF/ekf.h
@ -49,7 +49,10 @@ public:
|
||||
Ekf();
|
||||
~Ekf();
|
||||
|
||||
// initialise variables to sane values (also interface class)
|
||||
bool init(uint64_t timestamp);
|
||||
|
||||
// should be called every time new data is pushed into the filter
|
||||
bool update();
|
||||
|
||||
// gets the innovations of velocity and position measurements
|
||||
@ -92,7 +95,7 @@ private:
|
||||
static const uint8_t _k_num_states = 24;
|
||||
static constexpr float _k_earth_rate = 0.000072921f;
|
||||
|
||||
stateSample _state;
|
||||
stateSample _state; // state struct of the ekf running at the delayed time horizon
|
||||
|
||||
bool _filter_initialised;
|
||||
bool _earth_rate_initialised;
|
||||
@ -102,7 +105,7 @@ private:
|
||||
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
|
||||
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
|
||||
|
||||
uint64_t _time_last_fake_gps;
|
||||
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
|
||||
|
||||
uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec)
|
||||
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
|
||||
@ -111,9 +114,9 @@ private:
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
|
||||
Vector3f _earth_rate_NED;
|
||||
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
|
||||
|
||||
matrix::Dcm<float> _R_prev;
|
||||
matrix::Dcm<float> _R_prev; // transformation matrix from earth frame to body frame of previous ekf step
|
||||
|
||||
float P[_k_num_states][_k_num_states]; // state covariance matrix
|
||||
|
||||
@ -126,11 +129,11 @@ private:
|
||||
float _heading_innov_var; // heading measurement innovation variance
|
||||
|
||||
// complementary filter states
|
||||
Vector3f _delta_angle_corr;
|
||||
Vector3f _delta_vel_corr;
|
||||
Vector3f _vel_corr;
|
||||
imuSample _imu_down_sampled;
|
||||
Quaternion _q_down_sampled;
|
||||
Vector3f _delta_angle_corr; // delta angle correction vector
|
||||
Vector3f _delta_vel_corr; // delta velocity correction vector
|
||||
Vector3f _vel_corr; // velocity correction vector
|
||||
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
|
||||
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
|
||||
@ -155,52 +158,65 @@ private:
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status;
|
||||
|
||||
// update the real time complementary filter states. This includes the prediction
|
||||
// and the correction step
|
||||
void calculateOutputStates();
|
||||
|
||||
// initialise filter states of both the delayed ekf and the real time complementary filter
|
||||
bool initialiseFilter(void);
|
||||
|
||||
// initialise ekf covariance matrix
|
||||
void initialiseCovariance();
|
||||
|
||||
// predict ekf state
|
||||
void predictState();
|
||||
|
||||
// predict ekf covariance
|
||||
void predictCovariance();
|
||||
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
void fuseMag();
|
||||
|
||||
// fuse magnetometer heading measurement
|
||||
void fuseHeading();
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
void fuseDeclination();
|
||||
|
||||
// fuse airspeed measurement
|
||||
void fuseAirspeed();
|
||||
|
||||
// fuse range measurements
|
||||
void fuseRange();
|
||||
|
||||
// fuse velocity and position measurements (also barometer height)
|
||||
void fuseVelPosHeight();
|
||||
|
||||
// reset velocity states of the ekf
|
||||
void resetVelocity();
|
||||
|
||||
// reset position states of the ekf (only vertical position)
|
||||
void resetPosition();
|
||||
|
||||
// reset height state of the ekf
|
||||
void resetHeight();
|
||||
|
||||
void makeCovSymetrical();
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
void limitCov();
|
||||
|
||||
void printCovToFile(char const *filename);
|
||||
|
||||
void assertCovNiceness();
|
||||
|
||||
// make ekf covariance matrix symmetric
|
||||
void makeSymmetrical();
|
||||
|
||||
// constrain the ekf states
|
||||
void constrainStates();
|
||||
|
||||
// generic function which will perform a fusion step given a kalman gain K
|
||||
// and a scalar innovation value
|
||||
void fuse(float *K, float innovation);
|
||||
|
||||
void printStates();
|
||||
|
||||
void printStatesFast();
|
||||
|
||||
// calculate the earth rotation vector from a given latitude
|
||||
void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
|
||||
|
||||
// return true id the GPS quality is good enough to set an origin and start aiding
|
||||
|
||||
@ -94,39 +94,6 @@ void Ekf::resetHeight()
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(__PX4_POSIX) && !defined(__PX4_QURT)
|
||||
void Ekf::printCovToFile(char const *filename)
|
||||
{
|
||||
std::ofstream myfile;
|
||||
myfile.open(filename);
|
||||
myfile << "Covariance matrix\n";
|
||||
myfile << std::setprecision(1);
|
||||
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
for (int j = 0; j < _k_num_states; j++) {
|
||||
myfile << std::to_string(P[i][j]) << std::setprecision(1) << " ";
|
||||
}
|
||||
|
||||
myfile << "\n\n\n\n\n\n\n\n\n\n";
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// This checks if the diagonal of the covariance matrix is non-negative
|
||||
// and that the matrix is symmetric
|
||||
void Ekf::assertCovNiceness()
|
||||
{
|
||||
for (int row = 0; row < _k_num_states; row++) {
|
||||
for (int column = 0; column < row; column++) {
|
||||
assert(fabsf(P[row][column] - P[column][row]) < 0.00001f);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
assert(P[i][i] > -0.000001f);
|
||||
}
|
||||
}
|
||||
|
||||
// This function forces the covariance matrix to be symmetric
|
||||
void Ekf::makeSymmetrical()
|
||||
{
|
||||
|
||||
@ -223,7 +223,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
|
||||
|
||||
_dt_imu_avg = 0.0f;
|
||||
_imu_time_last = timestamp;
|
||||
|
||||
_imu_sample_delayed.delta_ang.setZero();
|
||||
_imu_sample_delayed.delta_vel.setZero();
|
||||
@ -265,78 +264,3 @@ bool EstimatorInterface::position_is_valid()
|
||||
// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
|
||||
return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6;
|
||||
}
|
||||
|
||||
void EstimatorInterface::printStoredIMU()
|
||||
{
|
||||
printf("---------Printing IMU data buffer------------\n");
|
||||
|
||||
for (int i = 0; i < IMU_BUFFER_LENGTH; i++) {
|
||||
printIMU(&_imu_buffer[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorInterface::printIMU(struct imuSample *data)
|
||||
{
|
||||
printf("time %" PRIu64 "\n", data->time_us);
|
||||
printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt);
|
||||
printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt);
|
||||
printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2));
|
||||
printf("dV: %.5f %.5f %.5f \n\n", (double)data->delta_vel(0), (double)data->delta_vel(1), (double)data->delta_vel(2));
|
||||
}
|
||||
|
||||
void EstimatorInterface::printQuaternion(Quaternion &q)
|
||||
{
|
||||
printf("q1 %.5f q2 %.5f q3 %.5f q4 %.5f\n", (double)q(0), (double)q(1), (double)q(2), (double)q(3));
|
||||
}
|
||||
|
||||
void EstimatorInterface::print_imu_avg_time()
|
||||
{
|
||||
printf("dt_avg: %.5f\n", (double)_dt_imu_avg);
|
||||
}
|
||||
|
||||
void EstimatorInterface::printStoredMag()
|
||||
{
|
||||
printf("---------Printing mag data buffer------------\n");
|
||||
|
||||
for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
|
||||
printMag(&_mag_buffer[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorInterface::printMag(struct magSample *data)
|
||||
{
|
||||
printf("time %" PRIu64 "\n", data->time_us);
|
||||
printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2));
|
||||
|
||||
}
|
||||
|
||||
void EstimatorInterface::printBaro(struct baroSample *data)
|
||||
{
|
||||
printf("time %" PRIu64 "\n", data->time_us);
|
||||
printf("baro: %.5f\n\n", (double)data->hgt);
|
||||
}
|
||||
|
||||
void EstimatorInterface::printStoredBaro()
|
||||
{
|
||||
printf("---------Printing baro data buffer------------\n");
|
||||
|
||||
for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
|
||||
printBaro(&_baro_buffer[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorInterface::printGps(struct gpsSample *data)
|
||||
{
|
||||
printf("time %" PRIu64 "\n", data->time_us);
|
||||
printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt);
|
||||
printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2));
|
||||
}
|
||||
|
||||
void EstimatorInterface::printStoredGps()
|
||||
{
|
||||
printf("---------Printing GPS data buffer------------\n");
|
||||
|
||||
for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
|
||||
printGps(&_gps_buffer[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -128,17 +128,6 @@ public:
|
||||
// set vehicle arm status data
|
||||
void set_arm_status(bool data) { _vehicle_armed = data; }
|
||||
|
||||
void printIMU(struct imuSample *data);
|
||||
void printStoredIMU();
|
||||
void printQuaternion(Quaternion &q);
|
||||
void print_imu_avg_time();
|
||||
void printMag(struct magSample *data);
|
||||
void printStoredMag();
|
||||
void printBaro(struct baroSample *data);
|
||||
void printStoredBaro();
|
||||
void printGps(struct gpsSample *data);
|
||||
void printStoredGps();
|
||||
|
||||
bool position_is_valid();
|
||||
|
||||
|
||||
@ -162,22 +151,22 @@ public:
|
||||
}
|
||||
void copy_timestamp(uint64_t *time_us)
|
||||
{
|
||||
*time_us = _imu_time_last;
|
||||
*time_us = _time_last_imu;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
parameters _params; // filter parameters
|
||||
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 10;
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 30;
|
||||
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10;
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
|
||||
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
|
||||
|
||||
float _dt_imu_avg;
|
||||
uint64_t _imu_time_last;
|
||||
float _dt_imu_avg; // average imu update period in s
|
||||
|
||||
imuSample _imu_sample_delayed;
|
||||
imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon
|
||||
|
||||
// measurement samples capturing measurements on the delayed time horizon
|
||||
magSample _mag_sample_delayed;
|
||||
baroSample _baro_sample_delayed;
|
||||
gpsSample _gps_sample_delayed;
|
||||
@ -185,14 +174,14 @@ protected:
|
||||
airspeedSample _airspeed_sample_delayed;
|
||||
flowSample _flow_sample_delayed;
|
||||
|
||||
outputSample _output_sample_delayed;
|
||||
outputSample _output_new;
|
||||
imuSample _imu_sample_new;
|
||||
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
|
||||
|
||||
uint64_t _imu_ticks;
|
||||
uint64_t _imu_ticks; // counter for imu updates
|
||||
|
||||
bool _imu_updated = false;
|
||||
bool _initialised = false;
|
||||
bool _imu_updated = false; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised = false; // true if ekf interface instance (data buffering) is initialised
|
||||
bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground
|
||||
|
||||
bool _NED_origin_initialised = false;
|
||||
@ -206,6 +195,7 @@ protected:
|
||||
|
||||
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
|
||||
|
||||
// data buffer instances
|
||||
RingBuffer<imuSample> _imu_buffer;
|
||||
RingBuffer<gpsSample> _gps_buffer;
|
||||
RingBuffer<magSample> _mag_buffer;
|
||||
@ -215,15 +205,19 @@ protected:
|
||||
RingBuffer<flowSample> _flow_buffer;
|
||||
RingBuffer<outputSample> _output_buffer;
|
||||
|
||||
uint64_t _time_last_imu;
|
||||
uint64_t _time_last_gps;
|
||||
uint64_t _time_last_mag;
|
||||
uint64_t _time_last_baro;
|
||||
uint64_t _time_last_range;
|
||||
uint64_t _time_last_airspeed;
|
||||
uint64_t _time_last_imu; // timestamp of last imu sample in microseconds
|
||||
uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds
|
||||
uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds
|
||||
uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds
|
||||
uint64_t _time_last_range; // timestamp of last range measurement in microseconds
|
||||
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
|
||||
|
||||
|
||||
fault_status_t _fault_status;
|
||||
|
||||
// allocate data buffers and intialise interface variables
|
||||
bool initialise_interface(uint64_t timestamp);
|
||||
|
||||
// free buffer memory
|
||||
void unallocate_buffers();
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user