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

Merge pull request #2459 from wingtra/estimator_LP_rates_log_covariances 

EKF Enhancments: LP filters on rates and covariance added to logging
This commit is contained in:
Lorenz Meier 2015-06-25 10:45:14 +02:00
parent 475c28803e 66a637dcc7
commit 74bdec2cc6
7 changed files with 70 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/estimator_status.msg
View File

@ -4,3 +4,4 @@ float32 n_states # Number of states effectively used
uint8 nan_flags # Bitmask to indicate NaN states
uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt)
uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt)
float32[28] covariances # Diagonal Elements of Covariance Matrix

7
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -66,6 +66,8 @@
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
@ -258,6 +260,11 @@ private:
AttPosEKF *_ekf;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _LP_att_P;
math::LowPassFilter2p _LP_att_Q;
math::LowPassFilter2p _LP_att_R;
private:
/**
* Update our local parameter cache.

21
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_newRangeData(false),
_mavlink_fd(-1),
_parameters {},
_parameter_handles {},
_ekf(nullptr)
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_LP_att_P(100.0f, 10.0f),
_LP_att_Q(100.0f, 10.0f),
_LP_att_R(100.0f, 10.0f)
{
_last_run = hrt_absolute_time();
@ -474,8 +478,13 @@ int AttitudePositionEstimatorEKF::check_filter_state()
size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
// Copy diagonal elemnts of covariance matrix
float covariances[28];
_ekf->get_covariance(covariances);
for (size_t i = 0; i < rep.n_states; i++) {
rep.states[i] = ekf_report.states[i];
rep.covariances[i] = covariances[i];
}
@ -819,9 +828,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;

7
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -3337,3 +3337,10 @@ void AttPosEKF::setIsFixedWing(const bool fixedWing)
{
_isFixedWing = fixedWing;
}
void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES])
{
for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) {
c[i] = P[i][i];
}
}

2
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -379,6 +379,8 @@ public:
*/
void ZeroVariables();
void get_covariance(float c[28]);
protected:
/**

14
src/modules/sdlog2/sdlog2.c
View File

@ -1126,6 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_TEL_s log_TEL;
struct log_EST0_s log_EST0;
struct log_EST1_s log_EST1;
struct log_EST2_s log_EST2;
struct log_EST3_s log_EST3;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_VISN_s log_VISN;
@ -1845,6 +1847,18 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
LOGBUFFER_WRITE_AND_COUNT(EST1);
log_msg.msg_type = LOG_EST2_MSG;
unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov);
memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov));
memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
LOGBUFFER_WRITE_AND_COUNT(EST2);
log_msg.msg_type = LOG_EST3_MSG;
unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov);
memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov));
memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3);
LOGBUFFER_WRITE_AND_COUNT(EST3);
}
/* --- TECS STATUS --- */

34
src/modules/sdlog2/sdlog2_messages.h
View File

@ -402,11 +402,23 @@ struct log_EST1_s {
float s[16];
};
/* --- EST2 - ESTIMATOR STATUS --- */
#define LOG_EST2_MSG 34
struct log_EST2_s {
float cov[12];
};
/* --- EST3 - ESTIMATOR STATUS --- */
#define LOG_EST3_MSG 35
struct log_EST3_s {
float cov[16];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 34
#define LOG_TEL1_MSG 35
#define LOG_TEL2_MSG 36
#define LOG_TEL3_MSG 37
#define LOG_TEL0_MSG 36
#define LOG_TEL1_MSG 37
#define LOG_TEL2_MSG 38
#define LOG_TEL3_MSG 39
struct log_TEL_s {
uint8_t rssi;
uint8_t remote_rssi;
@ -419,7 +431,7 @@ struct log_TEL_s {
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 38
#define LOG_VISN_MSG 40
struct log_VISN_s {
float x;
float y;
@ -434,7 +446,7 @@ struct log_VISN_s {
};
/* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 39
#define LOG_ENCD_MSG 41
struct log_ENCD_s {
int64_t cnt0;
float vel0;
@ -443,22 +455,22 @@ struct log_ENCD_s {
};
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
#define LOG_AIR1_MSG 41
#define LOG_AIR1_MSG 42
/* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 42
#define LOG_VTOL_MSG 43
struct log_VTOL_s {
float airspeed_tot;
};
/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
#define LOG_TSYN_MSG 43
#define LOG_TSYN_MSG 44
struct log_TSYN_s {
uint64_t time_offset;
};
/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
#define LOG_MACS_MSG 44
#define LOG_MACS_MSG 45
struct log_MACS_s {
float roll_rate_integ;
float pitch_rate_integ;
@ -522,6 +534,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"),
LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),