mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
Mirs2605Ekf.cpp
[詳解]
1#include "Mirs2605Ekf.hpp"
2#include <math.h>
3
4namespace {
5
6float clampDt(float dt) {
7 if (dt <= 0.0f) {
8 return 0.0f;
9 }
10 if (dt > 0.1f) {
11 return 0.1f;
12 }
13 return dt;
14}
15
16} // namespace
17
21
23 for (int i = 0; i < N; ++i) {
24 x_[i] = 0.0f;
25 for (int j = 0; j < N; ++j) {
26 p_[i][j] = 0.0f;
27 }
28 }
29 // 起動直後は姿勢・バイアスが不定のため大きめに持つ
30 p_[IX][IX] = 1e-2f;
31 p_[IY][IY] = 1e-2f;
32 p_[ITH][ITH] = 1e-2f;
33 p_[IV][IV] = 1e-2f;
34 p_[IBG][IBG] = 1e-4f;
35}
36
37void Mirs2605Ekf::setProcessNoise(float q_theta, float q_v, float q_bg) {
38 if (q_theta > 0.0f) {
39 q_theta_ = q_theta;
40 }
41 if (q_v > 0.0f) {
42 q_v_ = q_v;
43 }
44 if (q_bg > 0.0f) {
45 q_bg_ = q_bg;
46 }
47}
48
49void Mirs2605Ekf::setMeasurementNoise(float r_vel, float r_yaw_rate, float r_mag) {
50 if (r_vel > 0.0f) {
51 r_vel_ = r_vel;
52 }
53 if (r_yaw_rate > 0.0f) {
54 r_yaw_rate_ = r_yaw_rate;
55 }
56 if (r_mag > 0.0f) {
57 r_mag_ = r_mag;
58 }
59}
60
61void Mirs2605Ekf::setInitialCovariance(float p_xy, float p_theta, float p_v, float p_bg) {
62 p_[IX][IX] = p_xy;
63 p_[IY][IY] = p_xy;
64 p_[ITH][ITH] = p_theta;
65 p_[IV][IV] = p_v;
66 p_[IBG][IBG] = p_bg;
67}
68
69float Mirs2605Ekf::cov(int i, int j) const {
70 if (i < 0 || i >= N || j < 0 || j >= N) {
71 return 0.0f;
72 }
73 return p_[i][j];
74}
75
76void Mirs2605Ekf::predict(float gz_rad_s, float dt_sec) {
77 const float dt = clampDt(dt_sec);
78 if (dt <= 0.0f) {
79 return;
80 }
81 const float theta = x_[ITH];
82 const float v = x_[IV];
83 const float w = gz_rad_s - x_[IBG];
84 const float c = cosf(theta);
85 const float s = sinf(theta);
86
87 // --- 状態伝搬 ---
88 x_[IX] += v * c * dt;
89 x_[IY] += v * s * dt;
90 x_[ITH] = normalizeAngle(theta + w * dt);
91 // v, bg はランダムウォーク(予測では維持)
92
93 // --- 共分散伝搬 P = F P F^T + Q ---
94 // F の非自明成分のみ展開する(5x5全乗算はESP32でも軽いが自明項を省く)
95 const float f02 = -v * s * dt;
96 const float f03 = c * dt;
97 const float f12 = v * c * dt;
98 const float f13 = s * dt;
99 const float f24 = -dt;
100
101 // FP = F * P
102 float fp[N][N];
103 for (int j = 0; j < N; ++j) {
104 fp[0][j] = p_[0][j] + f02 * p_[2][j] + f03 * p_[3][j];
105 fp[1][j] = p_[1][j] + f12 * p_[2][j] + f13 * p_[3][j];
106 fp[2][j] = p_[2][j] + f24 * p_[4][j];
107 fp[3][j] = p_[3][j];
108 fp[4][j] = p_[4][j];
109 }
110 // P = FP * F^T
111 float np[N][N];
112 for (int i = 0; i < N; ++i) {
113 np[i][0] = fp[i][0] + fp[i][2] * f02 + fp[i][3] * f03;
114 np[i][1] = fp[i][1] + fp[i][2] * f12 + fp[i][3] * f13;
115 np[i][2] = fp[i][2] + fp[i][4] * f24;
116 np[i][3] = fp[i][3];
117 np[i][4] = fp[i][4];
118 }
119 // 対称化+Q加算(数値誤差の蓄積を抑える)
120 for (int i = 0; i < N; ++i) {
121 for (int j = 0; j < N; ++j) {
122 p_[i][j] = 0.5f * (np[i][j] + np[j][i]);
123 }
124 }
125 p_[ITH][ITH] += q_theta_ * dt;
126 p_[IV][IV] += q_v_ * dt;
127 p_[IBG][IBG] += q_bg_ * dt;
128}
129
130void Mirs2605Ekf::scalarUpdate(const float h[N], float residual, float r) {
131 if (!(r > 0.0f)) {
132 return;
133 }
134 // S = H P H^T + R
135 float hp[N] = {0.0f};
136 for (int j = 0; j < N; ++j) {
137 float acc = 0.0f;
138 for (int k = 0; k < N; ++k) {
139 acc += h[k] * p_[k][j];
140 }
141 hp[j] = acc;
142 }
143 float s = r;
144 for (int k = 0; k < N; ++k) {
145 s += hp[k] * h[k];
146 }
147 if (!(s > 1e-12f)) {
148 return;
149 }
150 // K = P H^T / S
151 float k[N];
152 for (int i = 0; i < N; ++i) {
153 float acc = 0.0f;
154 for (int kk = 0; kk < N; ++kk) {
155 acc += p_[i][kk] * h[kk];
156 }
157 k[i] = acc / s;
158 }
159 for (int i = 0; i < N; ++i) {
160 x_[i] += k[i] * residual;
161 }
162 x_[ITH] = normalizeAngle(x_[ITH]);
163 // P = (I - K H) P(Joseph形の簡易版。逐次スカラ更新では十分)
164 float kh[N][N];
165 for (int i = 0; i < N; ++i) {
166 for (int j = 0; j < N; ++j) {
167 kh[i][j] = k[i] * hp[j];
168 }
169 }
170 for (int i = 0; i < N; ++i) {
171 for (int j = 0; j < N; ++j) {
172 p_[i][j] -= kh[i][j];
173 }
174 // 対角が負に潰れないよう下限を保つ
175 if (p_[i][i] < 1e-12f) {
176 p_[i][i] = 1e-12f;
177 }
178 }
179}
180
181void Mirs2605Ekf::updateVel(float v_enc, float r_override) {
182 const float h[N] = {0.0f, 0.0f, 0.0f, 1.0f, 0.0f};
183 scalarUpdate(h, v_enc - x_[IV], r_override > 0.0f ? r_override : r_vel_);
184}
185
186void Mirs2605Ekf::updateYawRate(float w_enc, float gz_rad_s, float r_override) {
187 // z = gz - w_enc を bg の観測とする
188 const float h[N] = {0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
189 scalarUpdate(h, (gz_rad_s - w_enc) - x_[IBG], r_override > 0.0f ? r_override : r_yaw_rate_);
190}
191
192void Mirs2605Ekf::updateYawRateGated(float w_enc, float gz_rad_s, float v_enc,
193 float v_gate, float r_override) {
194 if (!(v_gate > 0.0f)) {
195 return;
196 }
197 float av = v_enc;
198 if (av < 0.0f) {
199 av = -av;
200 }
201 if (av > v_gate) {
202 return;
203 }
204 updateYawRate(w_enc, gz_rad_s, r_override);
205}
206
207void Mirs2605Ekf::updateMagTheta(float yaw_mag_rad, float r_override) {
208 const float h[N] = {0.0f, 0.0f, 1.0f, 0.0f, 0.0f};
209 scalarUpdate(h, angleResidual(yaw_mag_rad, x_[ITH]), r_override > 0.0f ? r_override : r_mag_);
210}
211
212void Mirs2605Ekf::updateZeroVel(float r_override) {
213 const float h[N] = {0.0f, 0.0f, 0.0f, 1.0f, 0.0f};
214 scalarUpdate(h, 0.0f - x_[IV], r_override > 0.0f ? r_override : r_zupt_);
215}
216
218 return atan2f(sinf(a), cosf(a));
219}
220
221float Mirs2605Ekf::angleResidual(float meas, float pred) {
222 return normalizeAngle(meas - pred);
223}
224
225float Mirs2605Ekf::magYawTiltCompensated(float mx, float my, float mz,
226 float ax, float ay, float az) {
227 const float norm = sqrtf(ax * ax + ay * ay + az * az);
228 if (!(norm > 1e-6f)) {
229 return atan2f(-my, mx);
230 }
231 // 正規化加速度から roll/pitch を求め、地磁気を水平面に戻す
232 const float axn = ax / norm;
233 const float ayn = ay / norm;
234 const float azn = az / norm;
235 const float roll = atan2f(ayn, azn);
236 const float pitch = atan2f(-axn, sqrtf(ayn * ayn + azn * azn));
237 const float cr = cosf(roll);
238 const float sr = sinf(roll);
239 const float cp = cosf(pitch);
240 const float sp = sinf(pitch);
241 const float mxh = mx * cp + mz * sp;
242 const float myh = mx * sr * sp + my * cr - mz * sr * cp;
243 return atan2f(-myh, mxh);
244}
245
246bool Mirs2605Ekf::isStationary(float ax, float ay, float az,
247 float gz_rad_s, float v_enc,
248 float acc_tol, float gz_tol, float v_tol) {
249 const float norm = sqrtf(ax * ax + ay * ay + az * az);
250 constexpr float kGravity = 9.80665f;
251 if (fabsf(norm - kGravity) > acc_tol) {
252 return false;
253 }
254 if (fabsf(gz_rad_s) > gz_tol) {
255 return false;
256 }
257 if (fabsf(v_enc) > v_tol) {
258 return false;
259 }
260 return true;
261}
車輪とIMUの値から、計測のばらつきを考慮して機体の位置・向きを推定する。
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
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