fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Add terrain vertical position state to prediction methods

Browse Source
This commit is contained in:
Paul Riseborough
2022-01-11 20:12:53 +11:00
committed by bresch
parent 7269a8ec3e
commit 294e3abfeb
22 changed files with 175 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+2 -2
View File
@@ -116,14 +116,14 @@ void Ekf::fuseAirspeed()
_fault_status.flags.bad_airspeed = false;
// Observation Jacobians
SparseVector24f<4,5,6,22,23> Hfusion;
SparseVector25f<4,5,6,22,23> Hfusion;
Hfusion.at<4>() = HK4;
Hfusion.at<5>() = HK5;
Hfusion.at<6>() = HK3*vd;
Hfusion.at<22>() = -HK4;
Hfusion.at<23>() = -HK5;
Vector24f Kfusion; // Kalman gain vector
Vector25f Kfusion; // Kalman gain vector
if (!update_wind_only) {
// we have no other source of aiding, so use airspeed measurements to correct states
src/modules/ekf2/EKF/common.h
+7 -6
View File
@@ -386,14 +386,15 @@ struct parameters {
};
struct stateSample {
Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame
Vector3f vel; ///< NED velocity in earth frame in m/s
Vector3f pos; ///< NED position in earth frame in m
Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame
Vector3f vel; ///< NED velocity in earth frame in m/s
Vector3f pos; ///< NED position in earth frame in m
Vector3f delta_ang_bias; ///< delta angle bias estimate in rad
Vector3f delta_vel_bias; ///< delta velocity bias estimate in m/s
Vector3f mag_I; ///< NED earth magnetic field in gauss
Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss
Vector2f wind_vel; ///< horizontal wind velocity in earth frame in m/s
Vector3f mag_I; ///< NED earth magnetic field in gauss
Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss
Vector2f wind_vel; ///< horizontal wind velocity in earth frame in m/s
float posd_terrain; ///< vertical position of the terrain surface in m
};
union fault_status_u {
src/modules/ekf2/EKF/covariance.cpp
+53 -4
View File
@@ -202,7 +202,7 @@ void Ekf::predictCovariance()
}
// compute noise variance for stationary processes
Vector24f process_noise;
Vector25f process_noise;
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
@@ -217,6 +217,9 @@ void Ekf::predictCovariance()
process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
// wind velocity states
process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
// terrain vertical position state
process_noise.slice<1, 1>(24, 0) = sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise) + sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
* (sq(_state.vel(0)) + sq(_state.vel(1)));
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities
@@ -482,7 +485,7 @@ void Ekf::predictCovariance()
// covariance update
SquareMatrix24f nextP;
SquareMatrix25f nextP;
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
@@ -862,6 +865,37 @@ void Ekf::predictCovariance()
}
if (_terrain_initialised) {
nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13;
nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12;
nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11;
nextP(3,24) = P(0,24)*PS13 + P(1,24)*PS12 + P(10,24)*PS7 - P(11,24)*PS6 - P(12,24)*PS34 - P(2,24)*PS11 + P(3,24);
nextP(4,24) = P(0,24)*PS174 + P(1,24)*PS173 + P(13,24)*PS43 + P(14,24)*PS172 - P(15,24)*PS171 + P(2,24)*PS175 - P(3,24)*PS176 + P(4,24);
nextP(5,24) = -P(0,24)*PS202 - P(1,24)*PS204 - P(13,24)*PS193 + P(14,24)*PS75 + P(15,24)*PS190 + P(2,24)*PS201 + P(3,24)*PS203 + P(5,24);
nextP(6,24) = P(0,24)*PS216 + P(1,24)*PS217 + P(13,24)*PS199 - P(14,24)*PS197 + P(15,24)*PS87 - P(2,24)*PS214 + P(3,24)*PS215 + P(6,24);
nextP(7,24) = P(4,24)*dt + P(7,24);
nextP(8,24) = P(5,24)*dt + P(8,24);
nextP(9,24) = P(6,24)*dt + P(9,24);
nextP(10,24) = P(10,24);
nextP(11,24) = P(11,24);
nextP(12,24) = P(12,24);
nextP(13,24) = P(13,24);
nextP(14,24) = P(14,24);
nextP(15,24) = P(15,24);
nextP(16,24) = P(16,24);
nextP(17,24) = P(17,24);
nextP(18,24) = P(18,24);
nextP(19,24) = P(19,24);
nextP(20,24) = P(20,24);
nextP(21,24) = P(21,24);
nextP(22,24) = P(22,24);
nextP(23,24) = P(23,24);
nextP(24,24) = P(24,24);
// add process noise that is not from the IMU
nextP(24, 24) += process_noise(24);
}
// stop position covariance growth if our total position variance reaches 100m
// this can happen if we lose gps for some time
if ((P(7, 7) + P(8, 8)) > 1e4f) {
@@ -898,7 +932,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// and set corresponding entries in Q to zero when states exceed 50% of the limit
// Covariance diagonal limits. Use same values for states which
// belong to the same group (e.g. vel_x, vel_y, vel_z)
float P_lim[8] = {};
float P_lim[9] = {};
P_lim[0] = 1.0f; // quaternion max var
P_lim[1] = 1e6f; // velocity max var
P_lim[2] = 1e6f; // positiion max var
@@ -907,6 +941,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
P_lim[5] = 1.0f; // earth mag field max var
P_lim[6] = 1.0f; // body mag field max var
P_lim[7] = 1e6f; // wind max var
P_lim[8] = 1E4f; // terrain vertical position max variance
for (int i = 0; i <= 3; i++) {
// quaternion states
@@ -1056,11 +1091,25 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
P.makeRowColSymmetric<2>(22);
}
}
// terrain vertical position
if (!_terrain_initialised) {
P.uncorrelateCovarianceSetVariance<1>(24, 0.0f);
} else {
// constrain variances
P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]);
// force symmetry
if (force_symmetry) {
P.makeRowColSymmetric<1>(24);
}
}
}
// if the covariance correction will result in a negative variance, then
// the covariance matrix is unhealthy and must be corrected
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP)
{
bool healthy = true;
src/modules/ekf2/EKF/drag_fusion.cpp
+2 -2
View File
@@ -46,8 +46,8 @@
void Ekf::fuseDrag(const dragSample &drag_sample)
{
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians
Vector24f Kfusion; // Kalman gain vector
SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians
Vector25f Kfusion; // Kalman gain vector
const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
src/modules/ekf2/EKF/ekf.h
+14 -14
View File
@@ -51,15 +51,15 @@
class Ekf final : public EstimatorInterface
{
public:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
static constexpr uint8_t _k_num_states{25}; ///< number of EKF states
typedef matrix::Vector<float, _k_num_states> Vector24f;
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
typedef matrix::Vector<float, _k_num_states> Vector25f;
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix25f;
typedef matrix::SquareMatrix<float, 2> Matrix2f;
typedef matrix::Vector<float, 4> Vector4f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<25, Idxs...>;
Ekf() = default;
virtual ~Ekf() = default;
@@ -125,7 +125,7 @@ public:
void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
matrix::Vector<float, 25> getStateAtFusionHorizonAsVector() const;
// get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
@@ -137,10 +137,10 @@ public:
float getTrueAirspeed() const;
// get the full covariance matrix
const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
const matrix::SquareMatrix<float, 25> &covariances() const { return P; }
// get the diagonal elements of the covariance matrix
matrix::Vector<float, 24> covariances_diagonal() const { return P.diag(); }
matrix::Vector<float, 25> covariances_diagonal() const { return P.diag(); }
// get the orientation (quaterion) covariances
matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }
@@ -418,7 +418,7 @@ private:
bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited
SquareMatrix24f P{}; ///< state covariance matrix
SquareMatrix25f P{}; ///< state covariance matrix
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
@@ -739,9 +739,9 @@ private:
// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
// that is optimized by exploring the sparsity in H
template <size_t ...Idxs>
SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
SquareMatrix25f computeKHP(const Vector25f &K, const SparseVector25f<Idxs...> &H) const
{
SquareMatrix24f KHP;
SquareMatrix25f KHP;
constexpr size_t non_zeros = sizeof...(Idxs);
float KH[non_zeros];
@@ -768,7 +768,7 @@ private:
// measurement update with a single measurement
// returns true if fusion is performed
template <size_t ...Idxs>
bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
bool measurementUpdate(Vector25f &K, const SparseVector25f<Idxs...> &H, float innovation)
{
for (unsigned i = 0; i < 3; i++) {
if (_accel_bias_inhibit[i]) {
@@ -779,7 +779,7 @@ private:
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(K, H);
const SquareMatrix25f KHP = computeKHP(K, H);
const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
@@ -798,7 +798,7 @@ private:
// if the covariance correction will result in a negative variance, then
// the covariance matrix is unhealthy and must be corrected
bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP);
bool checkAndFixCovarianceUpdate(const SquareMatrix25f &KHP);
// limit the diagonal of the covariance matrix
// force symmetry when the argument is true
@@ -809,7 +809,7 @@ private:
// generic function which will perform a fusion step given a kalman gain K
// and a scalar innovation value
void fuse(const Vector24f &K, float innovation);
void fuse(const Vector25f &K, float innovation);
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
src/modules/ekf2/EKF/ekf_helper.cpp
+4 -3
View File
@@ -673,9 +673,9 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
}
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
matrix::Vector<float, 25> Ekf::getStateAtFusionHorizonAsVector() const
{
matrix::Vector<float, 24> state;
matrix::Vector<float, 25> state;
state.slice<4, 1>(0, 0) = _state.quat_nominal;
state.slice<3, 1>(4, 0) = _state.vel;
state.slice<3, 1>(7, 0) = _state.pos;
@@ -684,6 +684,7 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
state.slice<3, 1>(16, 0) = _state.mag_I;
state.slice<3, 1>(19, 0) = _state.mag_B;
state.slice<2, 1>(22, 0) = _state.wind_vel;
state.slice<1, 1>(24, 0) = _state.posd_terrain;
return state;
}
@@ -986,7 +987,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
*status = soln_status.value;
}
void Ekf::fuse(const Vector24f &K, float innovation)
void Ekf::fuse(const Vector25f &K, float innovation)
{
_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
_state.quat_nominal.normalize();
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+2 -2
View File
@@ -170,7 +170,7 @@ void Ekf::fuseGpsYaw()
// calculate observation jacobian
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3> Hfusion;
SparseVector25f<0,1,2,3> Hfusion;
Hfusion.at<0>() = -HK16*HK18;
Hfusion.at<1>() = -HK18*HK24;
Hfusion.at<2>() = -HK18*HK25;
@@ -178,7 +178,7 @@ void Ekf::fuseGpsYaw()
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion;
Vector25f Kfusion;
Kfusion(0) = HK27*HK32;
Kfusion(1) = HK28*HK32;
Kfusion(2) = HK30*HK32;
src/modules/ekf2/EKF/mag_fusion.cpp
+6 -6
View File
@@ -196,8 +196,8 @@ void Ekf::fuseMag(const Vector3f &mag)
const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
Vector24f Kfusion;
SparseVector25f<0,1,2,3,16,17,18,19,20,21> Hfusion;
Vector25f Kfusion;
// update the states and covariance using sequential fusion of the magnetometer components
for (uint8_t index = 0; index <= 2; index++) {
@@ -623,7 +623,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion;
Vector25f Kfusion;
for (uint8_t row = 0; row <= 15; row++) {
for (uint8_t col = 0; col <= 3; col++) {
@@ -679,7 +679,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
SquareMatrix24f KHP;
SquareMatrix25f KHP;
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) {
@@ -809,12 +809,12 @@ void Ekf::fuseDeclination(float decl_sigma)
// Calculate the observation Jacobian
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
// Note Hfusion indices do not match state indices
SparseVector24f<16,17> Hfusion;
SparseVector25f<16,17> Hfusion;
Hfusion.at<16>() = -HK0*HK2*magE;
Hfusion.at<17>() = HK4;
// Calculate the Kalman gains
Vector24f Kfusion;
Vector25f Kfusion;
for (unsigned row = 0; row <= 15; row++) {
Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
src/modules/ekf2/EKF/optflow_fusion.cpp
+2 -2
View File
@@ -260,8 +260,8 @@ void Ekf::fuseOptFlow()
}
// fuse observation axes sequentially
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector24f Kfusion; // Optical flow Kalman gains
SparseVector25f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector25f Kfusion; // Optical flow Kalman gains
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp
+8 -8
View File
@@ -3,8 +3,8 @@
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
float sq(float in) {
return in * in;
@@ -15,8 +15,8 @@ int main()
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
float Hfusion[24];
Vector24f H_MAG;
Vector24f Kfusion;
Vector25f H_MAG;
Vector25f Kfusion;
float mag_innov_var;
// quaternion inputs must be normalised
@@ -38,7 +38,7 @@ int main()
const bool update_all_states = true;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -132,7 +132,7 @@ int main()
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
Vector25f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
@@ -308,7 +308,7 @@ int main()
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
Vector25f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
@@ -488,7 +488,7 @@ int main()
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
Vector25f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp
+11 -11
View File
@@ -4,25 +4,25 @@
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
Vector24f H_ACC;
Vector24f Kfusion;
SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
Vector25f H_ACC;
Vector25f Kfusion;
float drag_innov_var;
Vector24f Hfusion_sympy;
Vector24f Kfusion_sympy;
Vector25f Hfusion_sympy;
Vector25f Kfusion_sympy;
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
Vector25f Hfusion_matlab;
Vector25f Kfusion_matlab;
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
@@ -56,7 +56,7 @@ int main()
const float rho = 1.225f;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp
+12 -12
View File
@@ -4,19 +4,19 @@
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
Vector24f Hfusion_sympy;
Vector24f Kfusion_sympy;
Vector25f Hfusion_sympy;
Vector25f Kfusion_sympy;
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
Vector25f Hfusion_matlab;
Vector25f Kfusion_matlab;
float _beta_innov_var;
@@ -43,7 +43,7 @@ int main()
const float vwe = 5.0f * 2.0f * ((float)rand() - 0.5f);
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -157,7 +157,7 @@ int main()
// }
// Observation Jacobians
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
Hfusion.at<0>() = HK21*HK22;
Hfusion.at<1>() = HK22*HK25;
Hfusion.at<2>() = HK22*HK26;
@@ -169,7 +169,7 @@ int main()
Hfusion.at<23>() = HK16*HK32;
// Calculate Kalman gains
Vector24f Kfusion;
Vector25f Kfusion;
if (!update_wind_only) {
Kfusion(0) = HK38*HK52;
@@ -224,7 +224,7 @@ int main()
SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd;
SH_BETA[12] = 2.0f*q0*q3;
Vector24f H_BETA;
Vector25f H_BETA;
H_BETA(0) = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
H_BETA(1) = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
H_BETA(2) = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
@@ -248,7 +248,7 @@ int main()
SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
Vector24f Kfusion;
Vector25f Kfusion;
Kfusion(0) = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]);
Kfusion(1) = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]);
Kfusion(2) = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]);
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
+7 -7
View File
@@ -1,9 +1,9 @@
// Equations for covariance matrix prediction, without process noise!
const float PS0 = powf(q1, 2);
const float PS0 = ecl::powf(q1, 2);
const float PS1 = 0.25F*daxVar;
const float PS2 = powf(q2, 2);
const float PS2 = ecl::powf(q2, 2);
const float PS3 = 0.25F*dayVar;
const float PS4 = powf(q3, 2);
const float PS4 = ecl::powf(q3, 2);
const float PS5 = 0.25F*dazVar;
const float PS6 = 0.5F*q1;
const float PS7 = 0.5F*q2;
@@ -94,7 +94,7 @@ const float PS91 = PS59 + PS82;
const float PS92 = PS69 + PS79;
const float PS93 = PS49 - 2*PS51 + PS53;
const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9;
const float PS95 = powf(q0, 2);
const float PS95 = ecl::powf(q0, 2);
const float PS96 = -P(10,11)*PS34;
const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96;
const float PS98 = P(0,2)*PS13;
@@ -238,20 +238,20 @@ nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS
nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122;
nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147;
nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167;
nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar;
nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*ecl::powf(PS56, 2) + PS185*ecl::powf(PS45, 2) + PS186 + ecl::powf(PS43, 2)*dvxVar;
nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86;
nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124;
nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149;
nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169;
nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196;
nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar;
nextP(5,5) = PS185*ecl::powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*ecl::powf(PS76, 2) + PS213 + ecl::powf(PS75, 2)*dvyVar;
nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94;
nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125;
nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150;
nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170;
nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200;
nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218;
nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar;
nextP(6,6) = PS184*ecl::powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*ecl::powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + ecl::powf(PS87, 2)*dvzVar;
nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt;
nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt;
nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt;
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated_compare.cpp
+4 -4
View File
@@ -3,7 +3,7 @@
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
inline float sq(float in) {
return in * in;
@@ -73,7 +73,7 @@ int main()
const float dvzVar = dvxVar;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -273,7 +273,7 @@ int main()
const float PS185 = P(0,13)*PS60 + P(1,13)*PS74 + P(13,13)*PS166 - P(13,14)*PS165 - P(13,15)*PS70 - P(2,13)*PS72 + P(3,13)*PS62 + P(6,13);
const float PS186 = P(0,6)*PS60 + P(1,6)*PS74 - P(2,6)*PS72 + P(3,6)*PS62 + P(6,13)*PS166 - P(6,14)*PS165 - P(6,15)*PS70 + P(6,6);
SquareMatrix24f nextP;
SquareMatrix25f nextP;
nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
@@ -577,7 +577,7 @@ int main()
nextP(23,23) = P(23,23);
// save output and repeat calculation using legacy matlab generated code
SquareMatrix24f nextP_sympy;
SquareMatrix25f nextP_sympy;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
nextP_sympy(row,col) = nextP(row,col);
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp
+10 -10
View File
@@ -4,24 +4,24 @@
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
int main()
{
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector24f Kfusion; // Optical flow observation Kalman gains
SparseVector25f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
Vector25f Kfusion; // Optical flow observation Kalman gains
Vector24f Hfusion_sympy;
Vector24f Kfusion_sympy;
Vector25f Hfusion_sympy;
Vector25f Kfusion_sympy;
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
Vector25f Hfusion_matlab;
Vector25f Kfusion_matlab;
const float R_LOS = sq(0.15f);
@@ -49,7 +49,7 @@ int main()
Tbs.identity();
// create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp
+7 -7
View File
@@ -3,10 +3,10 @@
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
float sq(float in) {
return in * in;
@@ -17,7 +17,7 @@ int main()
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
float H_YAW[24];
Vector24f Kfusion;
Vector25f Kfusion;
float _heading_innov_var;
const float R_YAW = sq(0.3f);
@@ -36,7 +36,7 @@ int main()
q3 /= length;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -97,7 +97,7 @@ int main()
// calculate observation jacobian
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3> Hfusion;
SparseVector25f<0,1,2,3> Hfusion;
Hfusion.at<0>() = -HK16*HK18;
Hfusion.at<1>() = -HK18*HK24;
Hfusion.at<2>() = -HK18*HK25;
@@ -115,7 +115,7 @@ int main()
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24] = {};
Vector24f Kfusion_sympy;
Vector25f Kfusion_sympy;
Hfusion_sympy[0] = Hfusion.at<0>();
Hfusion_sympy[1] = Hfusion.at<1>();
Hfusion_sympy[2] = Hfusion.at<2>();
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp
+6 -6
View File
@@ -3,8 +3,8 @@
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
float sq(float in) {
return in * in;
@@ -15,8 +15,8 @@ int main()
// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
float Hfusion[24] = {};
Vector24f H_DECL;
Vector24f Kfusion;
Vector25f H_DECL;
Vector25f Kfusion;
float decl_innov_var;
const float R_DECL = sq(0.3f);
@@ -40,7 +40,7 @@ int main()
const float h_field_min = 1e-3f;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -95,7 +95,7 @@ int main()
// save output and repeat calculation using legacy matlab generated code
float Hfusion_sympy[24];
Vector24f Kfusion_sympy;
Vector25f Kfusion_sympy;
for (int row=0; row<24; row++) {
Hfusion_sympy[row] = Hfusion[row];
Kfusion_sympy(row) = Kfusion(row);
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp
+11 -11
View File
@@ -4,10 +4,10 @@
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector25f = matrix::SparseVectorf<24, Idxs...>;
int main()
{
@@ -15,13 +15,13 @@ int main()
float airspeed_innov_var;
Vector24f Kfusion; // Kalman gain vector
Vector25f Kfusion; // Kalman gain vector
Vector24f Hfusion_sympy;
Vector24f Kfusion_sympy;
Vector25f Hfusion_sympy;
Vector25f Kfusion_sympy;
Vector24f Hfusion_matlab;
Vector24f Kfusion_matlab;
Vector25f Hfusion_matlab;
Vector25f Kfusion_matlab;
const float R_TAS = sq(1.5f);
@@ -37,7 +37,7 @@ int main()
const float vwe = 3.0f;
// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
SquareMatrix24f P;
SquareMatrix25f P;
for (int col=0; col<=23; col++) {
for (int row=0; row<=col; row++) {
if (row == col) {
@@ -77,7 +77,7 @@ int main()
const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
// Observation Jacobians
SparseVector24f<4,5,6,22,23> Hfusion;
SparseVector25f<4,5,6,22,23> Hfusion;
Hfusion.at<4>() = HK4;
Hfusion.at<5>() = HK5;
Hfusion.at<6>() = HK3*vd;
@@ -121,7 +121,7 @@ int main()
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
// Observation Jacobian
Vector24f H_TAS = {};
Vector25f H_TAS = {};
H_TAS(4) = SH_TAS[2];
H_TAS(5) = SH_TAS[1];
H_TAS(6) = vd*SH_TAS[0];
src/modules/ekf2/EKF/python/ekf_derivation/generated/util.h
+1 -1
View File
@@ -1,6 +1,6 @@
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
inline float sq(float in) {
return in * in;
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp
+2 -2
View File
@@ -3,8 +3,8 @@
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
typedef matrix::Vector<float, 24> Vector25f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix25f;
float sq(float in) {
return in * in;
src/modules/ekf2/EKF/sideslip_fusion.cpp
+2 -2
View File
@@ -173,7 +173,7 @@ void Ekf::fuseSideslip()
}
// Observation Jacobians
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
SparseVector25f<0,1,2,3,4,5,6,22,23> Hfusion;
Hfusion.at<0>() = HK21*HK22;
Hfusion.at<1>() = HK22*HK25;
Hfusion.at<2>() = HK22*HK26;
@@ -185,7 +185,7 @@ void Ekf::fuseSideslip()
Hfusion.at<23>() = HK16*HK32;
// Calculate Kalman gains
Vector24f Kfusion;
Vector25f Kfusion;
if (!update_wind_only) {
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -183,7 +183,7 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
Vector25f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
@@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
Kfusion(row) = P(row, state_index) / innov_var;
}
SquareMatrix24f KHP;
SquareMatrix25f KHP;
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {