mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
Mirs2605Ekf.hpp
[詳解]
1
21
#pragma once
22
23
#include <stdint.h>
24
25
class
Mirs2605Ekf
{
26
public
:
27
enum
StateIdx
{
IX
= 0,
IY
= 1,
ITH
= 2,
IV
= 3,
IBG
= 4,
N
= 5 };
28
29
Mirs2605Ekf
();
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
108
private
:
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
};
Mirs2605Ekf::updateVel
void updateVel(float v_enc, float r_override=-1.0f)
エンコーダ前進速度で v を補正する。
定義
Mirs2605Ekf.cpp:181
Mirs2605Ekf::normalizeAngle
static float normalizeAngle(float a)
定義
Mirs2605Ekf.cpp:217
Mirs2605Ekf::isStationary
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付近かつ角速度・車輪速度が微小。
定義
Mirs2605Ekf.cpp:246
Mirs2605Ekf::updateYawRate
void updateYawRate(float w_enc, float gz_rad_s, float r_override=-1.0f)
エンコーダ角速度でジャイロバイアスを補正する。
定義
Mirs2605Ekf.cpp:186
Mirs2605Ekf::magYawTiltCompensated
static float magYawTiltCompensated(float mx, float my, float mz, float ax, float ay, float az)
加速度でチルト補正した地磁気ヨーを求める。
定義
Mirs2605Ekf.cpp:225
Mirs2605Ekf::updateMagTheta
void updateMagTheta(float yaw_mag_rad, float r_override=-1.0f)
地磁気ヨーで theta を補正する(屋内磁気外乱時は呼ばないこと)。
定義
Mirs2605Ekf.cpp:207
Mirs2605Ekf::reset
void reset()
定義
Mirs2605Ekf.cpp:22
Mirs2605Ekf::setInitialCovariance
void setInitialCovariance(float p_xy, float p_theta, float p_v, float p_bg)
初期共分散の対角を設定(起動直後の不確かさ)。
定義
Mirs2605Ekf.cpp:61
Mirs2605Ekf::setProcessNoise
void setProcessNoise(float q_theta, float q_v, float q_bg)
プロセスノイズ対角を設定(既定値は室内低速向け)。
定義
Mirs2605Ekf.cpp:37
Mirs2605Ekf::setMeasurementNoise
void setMeasurementNoise(float r_vel, float r_yaw_rate, float r_mag)
観測ノイズ(分散)を設定。
定義
Mirs2605Ekf.cpp:49
Mirs2605Ekf::predict
void predict(float gz_rad_s, float dt_sec)
予測ステップ。ジャイロで theta/x/y を伝搬する。
定義
Mirs2605Ekf.cpp:76
Mirs2605Ekf::theta
float theta() const
定義
Mirs2605Ekf.hpp:85
Mirs2605Ekf::updateZeroVel
void updateZeroVel(float r_override=-1.0f)
停止中拘束。v=0 の観測として扱う(ZUPT)。
定義
Mirs2605Ekf.cpp:212
Mirs2605Ekf::StateIdx
StateIdx
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::IV
@ IV
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::IX
@ IX
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::N
@ N
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::IBG
@ IBG
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::ITH
@ ITH
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::IY
@ IY
定義
Mirs2605Ekf.hpp:27
Mirs2605Ekf::cov
float cov(int i, int j) const
定義
Mirs2605Ekf.cpp:69
Mirs2605Ekf::Mirs2605Ekf
Mirs2605Ekf()
定義
Mirs2605Ekf.cpp:18
Mirs2605Ekf::gyroBias
float gyroBias() const
定義
Mirs2605Ekf.hpp:87
Mirs2605Ekf::y
float y() const
定義
Mirs2605Ekf.hpp:84
Mirs2605Ekf::x
float x() const
定義
Mirs2605Ekf.hpp:83
Mirs2605Ekf::angleResidual
static float angleResidual(float meas, float pred)
定義
Mirs2605Ekf.cpp:221
Mirs2605Ekf::updateYawRateGated
void updateYawRateGated(float w_enc, float gz_rad_s, float v_enc, float v_gate=0.05f, float r_override=-1.0f)
低速ゲート付きバイアス更新。
定義
Mirs2605Ekf.cpp:192
Mirs2605Ekf::v
float v() const
定義
Mirs2605Ekf.hpp:86
lib
Mirs2605Ekf
Mirs2605Ekf.hpp
構築:
1.18.0