6float clampDt(
float dt) {
23 for (
int i = 0; i <
N; ++i) {
25 for (
int j = 0; j <
N; ++j) {
53 if (r_yaw_rate > 0.0f) {
54 r_yaw_rate_ = r_yaw_rate;
70 if (i < 0 || i >=
N || j < 0 || j >=
N) {
77 const float dt = clampDt(dt_sec);
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);
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;
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];
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;
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]);
125 p_[
ITH][
ITH] += q_theta_ * dt;
126 p_[
IV][
IV] += q_v_ * dt;
127 p_[
IBG][
IBG] += q_bg_ * dt;
130void Mirs2605Ekf::scalarUpdate(
const float h[N],
float residual,
float r) {
135 float hp[
N] = {0.0f};
136 for (
int j = 0; j <
N; ++j) {
138 for (
int k = 0; k <
N; ++k) {
139 acc += h[k] * p_[k][j];
144 for (
int k = 0; k <
N; ++k) {
152 for (
int i = 0; i <
N; ++i) {
154 for (
int kk = 0; kk <
N; ++kk) {
155 acc += p_[i][kk] * h[kk];
159 for (
int i = 0; i <
N; ++i) {
160 x_[i] += k[i] * residual;
165 for (
int i = 0; i <
N; ++i) {
166 for (
int j = 0; j <
N; ++j) {
167 kh[i][j] = k[i] * hp[j];
170 for (
int i = 0; i <
N; ++i) {
171 for (
int j = 0; j <
N; ++j) {
172 p_[i][j] -= kh[i][j];
175 if (p_[i][i] < 1e-12f) {
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_);
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_);
193 float v_gate,
float r_override) {
194 if (!(v_gate > 0.0f)) {
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_);
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_);
218 return atan2f(sinf(a), cosf(a));
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);
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);
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) {
254 if (fabsf(gz_rad_s) > gz_tol) {
257 if (fabsf(v_enc) > v_tol) {
車輪と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 を伝搬する。
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)
低速ゲート付きバイアス更新。