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

ekf2: remove size in name of state vector and matrix types 

Then the state vector size can be changes without having to update the
name
This commit is contained in:
bresch 2023-09-20 10:27:21 +02:00 committed by Daniel Agar
parent 4f1682c3c8
commit ebf962bf68
15 changed files with 50 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -194,8 +194,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
_fault_status.flags.bad_airspeed = false;
Vector24f H; // Observation jacobian
Vector24f K; // Kalman gain vector
VectorState H; // Observation jacobian
VectorState K; // Kalman gain vector
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);

4
src/modules/ekf2/EKF/covariance.cpp
View File

@ -227,7 +227,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
// predict the covariance
SquareMatrix24f nextP;
SquareMatrixState nextP;
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
@ -511,7 +511,7 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
// 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 SquareMatrixState &KHP)
{
bool healthy = true;

4
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -85,7 +85,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
_state.vel(2));
const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth);
const float rel_wind_speed = rel_wind_body.norm();
const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector();
const VectorState state_vector_prev = getStateAtFusionHorizonAsVector();
Vector2f bcoef_inv;
@ -105,7 +105,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
bcoef_inv(1) = bcoef_inv(0);
}
Vector24f Kfusion;
VectorState Kfusion;
// perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {

26
src/modules/ekf2/EKF/ekf.h
View File

@ -60,12 +60,12 @@ enum class Likelihood { LOW, MEDIUM, HIGH };
class Ekf final : public EstimatorInterface
{
public:
typedef matrix::Vector<float, State::size> Vector24f;
typedef matrix::SquareMatrix<float, State::size> SquareMatrix24f;
typedef matrix::Vector<float, State::size> VectorState;
typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
typedef matrix::SquareMatrix<float, 2> Matrix2f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<State::size, Idxs...>;
using SparseVectorState = matrix::SparseVectorf<State::size, Idxs...>;
Ekf()
{
@ -304,7 +304,7 @@ public:
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
matrix::Vector<float, State::size> getStateAtFusionHorizonAsVector() const;
// get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
@ -624,7 +624,7 @@ private:
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
SquareMatrix24f P{}; ///< state covariance matrix
SquareMatrixState P{}; ///< state covariance matrix
#if defined(CONFIG_EKF2_DRAG_FUSION)
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
@ -807,8 +807,8 @@ private:
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
#if defined(CONFIG_EKF2_GNSS_YAW)
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
@ -972,7 +972,7 @@ private:
float getMagDeclination();
#endif // CONFIG_EKF2_MAGNETOMETER
void clearInhibitedStateKalmanGains(Vector24f &K) const
void clearInhibitedStateKalmanGains(VectorState &K) const
{
// gyro bias: states 10, 11, 12
for (unsigned i = 0; i < 3; i++) {
@ -1009,12 +1009,12 @@ private:
}
}
bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
{
clearInhibitedStateKalmanGains(K);
const Vector24f KS = K * innovation_variance;
SquareMatrix24f KHP;
const VectorState KS = K * innovation_variance;
SquareMatrixState KHP;
for (unsigned row = 0; row < State::size; row++) {
for (unsigned col = 0; col < State::size; col++) {
@ -1041,7 +1041,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 SquareMatrixState &KHP);
// limit the diagonal of the covariance matrix
// force symmetry when the argument is true
@ -1054,7 +1054,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 VectorState &K, float innovation);
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;

4
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -810,7 +810,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 VectorState &K, float innovation)
{
_state.quat_nominal -= K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * innovation;
_state.quat_nominal.normalize();
@ -909,7 +909,7 @@ Vector3f Ekf::calcRotVecVariances() const
float Ekf::getYawVar() const
{
Vector24f H_YAW;
VectorState H_YAW;
float yaw_var = 0.f;
computeYawInnovVarAndH(0.f, yaw_var, H_YAW);

8
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -64,7 +64,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
float heading_innov_var;
{
Vector24f H;
VectorState H;
sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
}
@ -91,7 +91,7 @@ void Ekf::fuseGpsYaw()
return;
}
Vector24f H;
VectorState H;
{
float heading_pred;
@ -102,7 +102,7 @@ void Ekf::fuseGpsYaw()
sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
}
const SparseVector24f<0,1,2,3> Hfusion(H);
const SparseVectorState<0,1,2,3> Hfusion(H);
// check if the innovation variance calculation is badly conditioned
if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) {
@ -131,7 +131,7 @@ void Ekf::fuseGpsYaw()
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance;
VectorState Kfusion = P * Hfusion / gnss_yaw.innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation);
_fault_status.flags.bad_hdg = !is_fused;

2
src/modules/ekf2/EKF/gravity_fusion.cpp
View File

@ -60,7 +60,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
// calculate kalman gains and innovation variances
Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)
Vector3f innovation_variance;
Vector24f Kx, Ky, Kz; // Kalman gain vectors
VectorState Kx, Ky, Kz; // Kalman gain vectors
sym::ComputeGravityInnovVarAndKAndH(
getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON,
&innovation, &innovation_variance, &Kx, &Ky, &Kz);

14
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -62,9 +62,9 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
Vector3f innov_var;
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
Vector24f H;
const Vector24f state_vector = getStateAtFusionHorizonAsVector();
SparseVectorState<0,1,2,3,16,17,18,19,20,21> Hfusion;
VectorState H;
const VectorState state_vector = getStateAtFusionHorizonAsVector();
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
Hfusion = H;
@ -214,7 +214,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
}
Vector24f Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index];
VectorState Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index];
if (!update_all_states) {
for (unsigned row = 0; row <= 15; row++) {
@ -263,7 +263,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
Vector24f H;
VectorState H;
float decl_pred;
float innovation_variance;
@ -277,10 +277,10 @@ bool Ekf::fuseDeclination(float decl_sigma)
return false;
}
SparseVector24f<16,17> Hfusion(H);
SparseVectorState<16,17> Hfusion(H);
// Calculate the Kalman gains
Vector24f Kfusion = P * Hfusion / innovation_variance;
VectorState Kfusion = P * Hfusion / innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);

2
src/modules/ekf2/EKF/mag_heading_control.cpp
View File

@ -119,7 +119,7 @@ void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common
aid_src.time_last_fuse = _time_delayed_us;
} else {
Vector24f H_YAW;
VectorState H_YAW;
computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW);
if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) {

12
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -77,10 +77,10 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
aid_src.observation_variance[0] = R_LOS;
aid_src.observation_variance[1] = R_LOS;
const Vector24f state_vector = getStateAtFusionHorizonAsVector();
const VectorState state_vector = getStateAtFusionHorizonAsVector();
Vector2f innov_var;
Vector24f H;
VectorState H;
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(aid_src.innovation_variance);
@ -98,10 +98,10 @@ void Ekf::fuseOptFlow()
// a positive offset in earth frame leads to a smaller height above the ground.
float range = predictFlowRange();
const Vector24f state_vector = getStateAtFusionHorizonAsVector();
const VectorState state_vector = getStateAtFusionHorizonAsVector();
Vector2f innov_var;
Vector24f H;
VectorState H;
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
@ -148,8 +148,8 @@ void Ekf::fuseOptFlow()
}
}
SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
SparseVectorState<0,1,2,3,4,5,6> Hfusion(H);
VectorState Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
fused[index] = true;

4
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -135,8 +135,8 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
_fault_status.flags.bad_sideslip = false;
Vector24f H; // Observation jacobian
Vector24f K; // Kalman gain vector
VectorState H; // Observation jacobian
VectorState K; // Kalman gain vector
sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);

4
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -195,7 +195,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
// Helper function that fuses a single velocity or position measurement
bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index)
{
Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used.
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < State::size; row++) {
@ -204,7 +204,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int s
clearInhibitedStateKalmanGains(Kfusion);
SquareMatrix24f KHP;
SquareMatrixState KHP;
for (unsigned row = 0; row < State::size; row++) {
for (unsigned column = 0; column < State::size; column++) {

8
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -41,13 +41,13 @@
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
{
Vector24f H_YAW;
VectorState H_YAW;
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
return fuseYaw(aid_src_status, H_YAW);
}
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW)
{
// define the innovation gate size
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
@ -75,7 +75,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_Y
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion;
VectorState Kfusion;
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
for (uint8_t row = 0; row < State::size; row++) {
@ -136,7 +136,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_Y
return false;
}
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
{
if (shouldUse321RotationSequence(_R_to_earth)) {
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);

2
src/modules/ekf2/EKF/zero_gyro_update.cpp
View File

@ -82,7 +82,7 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed)
void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index)
{
Vector24f K; // Kalman gain vector for any single observation - sequential fusion is used.
VectorState K; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 10;
// calculate kalman gain K = PHS, where S = 1/innovation variance

2
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
View File

@ -56,7 +56,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
aid_src_status.observation_variance = obs_var;
aid_src_status.innovation = 0.f;
Vector24f H_YAW;
VectorState H_YAW;
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);