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

ekf2: auto-generate state vector size constant

Browse Source
This commit is contained in:
bresch
2023-09-19 09:37:50 -04:00
committed by Daniel Agar
parent 8a9a303354
commit dac337efc4
8 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+7 -7
View File
@@ -60,14 +60,12 @@ enum class Likelihood { LOW, MEDIUM, HIGH };
class Ekf final : public EstimatorInterface
{
public:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
typedef matrix::Vector<float, _k_num_states> Vector24f;
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
typedef matrix::Vector<float, State::size> Vector24f;
typedef matrix::SquareMatrix<float, State::size> SquareMatrix24f;
typedef matrix::SquareMatrix<float, 2> Matrix2f;
template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
using SparseVector24f = matrix::SparseVectorf<State::size, Idxs...>;
Ekf()
{
@@ -82,6 +80,8 @@ public:
// should be called every time new data is pushed into the filter
bool update();
static uint8_t getNumberOfStates() { return State::size; }
void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
@@ -1016,8 +1016,8 @@ private:
const Vector24f KS = K * innovation_variance;
SquareMatrix24f KHP;
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned col = 0; col < _k_num_states; col++) {
for (unsigned row = 0; row < State::size; row++) {
for (unsigned col = 0; col < State::size; col++) {
// Instad of literally computing KHP, use an equvalent
// equation involving less mathematical operations
KHP(row, col) = KS(row) * K(col);