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

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

Browse Source 
Then the state vector size can be changes without having to update the
name
This commit is contained in:
bresch
2023-09-21 08:27:52 -04:00
committed by Daniel Agar
parent 4f1682c3c8
commit ebf962bf68
15 changed files with 50 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+4 -4
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;