fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 04:33: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/yaw_fusion.cpp
+4 -4
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);