diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-17 20:49:46 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-17 20:49:46 +0200 |
commit | 171857811f76fb4d8fb3b65329345dd5af0ec9db (patch) | |
tree | 0c5ddb8ca9d3a17e586c8795b5af5db709c1621f /src/modules/ekf_att_pos_estimator/estimator_23states.h | |
parent | 60706500df4e5264f77d0a7c36edb7b6b60e21ac (diff) | |
download | px4-firmware-171857811f76fb4d8fb3b65329345dd5af0ec9db.tar.gz px4-firmware-171857811f76fb4d8fb3b65329345dd5af0ec9db.tar.bz2 px4-firmware-171857811f76fb4d8fb3b65329345dd5af0ec9db.zip |
EKF filter update. Now correctly scaling variance (well, if you could call this ever "correct") for repeated fusion of the same data quantity.
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_23states.h')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.h | 17 |
1 files changed, 16 insertions, 1 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index faa6735ca..12cbc53b8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -116,6 +116,9 @@ public: float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + // Times + uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement @@ -140,7 +143,12 @@ public: Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter + float dtIMUfilt; // average time between IMU measurements (sec) + float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter + float dtVelPosFilt; // average time between position / velocity fusion steps + float dtHgtFilt; // average time between height measurement updates + float dtGpsFilt; // average time between gps measurement updates uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -211,6 +219,9 @@ public: unsigned storeIndex; +void updateDtGpsFilt(float dt); + +void updateDtHgtFilt(float dt); void UpdateStrapdownEquationsNED(); @@ -300,6 +311,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination); protected: +void updateDtVelPosFilt(float dt); + bool FilterHealthy(); bool GyroOffsetsDiverged(); @@ -314,3 +327,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl uint32_t millis(); +uint64_t getMicros(); + |