mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
Mirs2605Ekf.hpp
[詳解]
1
21#pragma once
22
23#include <stdint.h>
24
26public:
27 enum StateIdx { IX = 0, IY = 1, ITH = 2, IV = 3, IBG = 4, N = 5 };
28
30
31 void reset();
32
38 void setProcessNoise(float q_theta, float q_v, float q_bg);
39
45 void setMeasurementNoise(float r_vel, float r_yaw_rate, float r_mag);
46
48 void setInitialCovariance(float p_xy, float p_theta, float p_v, float p_bg);
49
55 void predict(float gz_rad_s, float dt_sec);
56
58 void updateVel(float v_enc, float r_override = -1.0f);
59
65 void updateYawRate(float w_enc, float gz_rad_s, float r_override = -1.0f);
66
73 void updateYawRateGated(float w_enc, float gz_rad_s, float v_enc,
74 float v_gate = 0.05f, float r_override = -1.0f);
75
77 void updateMagTheta(float yaw_mag_rad, float r_override = -1.0f);
78
80 void updateZeroVel(float r_override = -1.0f);
81
82 // --- 状態取得 ---
83 float x() const { return x_[IX]; }
84 float y() const { return x_[IY]; }
85 float theta() const { return x_[ITH]; }
86 float v() const { return x_[IV]; }
87 float gyroBias() const { return x_[IBG]; }
88 float cov(int i, int j) const;
89
90 // --- 静的ヘルパ(Arduino非依存・単体テスト可) ---
91 static float normalizeAngle(float a);
92 static float angleResidual(float meas, float pred);
93
98 static float magYawTiltCompensated(float mx, float my, float mz,
99 float ax, float ay, float az);
100
102 static bool isStationary(float ax, float ay, float az,
103 float gz_rad_s, float v_enc,
104 float acc_tol = 0.6f,
105 float gz_tol = 0.03f,
106 float v_tol = 0.02f);
107
108private:
109 float x_[N];
110 float p_[N][N];
111 float q_theta_ = 1e-5f;
112 float q_v_ = 1e-3f;
113 float q_bg_ = 1e-8f;
114 float r_vel_ = 4e-4f; // sigma 0.02 m/s
115 float r_yaw_rate_ = 1e-4f; // sigma 0.01 rad/s
116 float r_mag_ = 9e-2f; // sigma 0.3 rad(屋内想定で大きめ)
117 float r_zupt_ = 1e-6f;
118
119 void scalarUpdate(const float h[N], float residual, float r);
120};
void updateVel(float v_enc, float r_override=-1.0f)
エンコーダ前進速度で v を補正する。
static float normalizeAngle(float a)
static bool isStationary(float ax, float ay, float az, float gz_rad_s, float v_enc, float acc_tol=0.6f, float gz_tol=0.03f, float v_tol=0.02f)
停止判定。加速度ノルムがg付近かつ角速度・車輪速度が微小。
void updateYawRate(float w_enc, float gz_rad_s, float r_override=-1.0f)
エンコーダ角速度でジャイロバイアスを補正する。
static float magYawTiltCompensated(float mx, float my, float mz, float ax, float ay, float az)
加速度でチルト補正した地磁気ヨーを求める。
void updateMagTheta(float yaw_mag_rad, float r_override=-1.0f)
地磁気ヨーで theta を補正する(屋内磁気外乱時は呼ばないこと)。
void setInitialCovariance(float p_xy, float p_theta, float p_v, float p_bg)
初期共分散の対角を設定(起動直後の不確かさ)。
void setProcessNoise(float q_theta, float q_v, float q_bg)
プロセスノイズ対角を設定(既定値は室内低速向け)。
void setMeasurementNoise(float r_vel, float r_yaw_rate, float r_mag)
観測ノイズ(分散)を設定。
void predict(float gz_rad_s, float dt_sec)
予測ステップ。ジャイロで theta/x/y を伝搬する。
float theta() const
void updateZeroVel(float r_override=-1.0f)
停止中拘束。v=0 の観測として扱う(ZUPT)。
float cov(int i, int j) const
float gyroBias() const
float y() const
float x() const
static float angleResidual(float meas, float pred)
void updateYawRateGated(float w_enc, float gz_rad_s, float v_enc, float v_gate=0.05f, float r_override=-1.0f)
低速ゲート付きバイアス更新。
float v() const