mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
PoseEstimator.cpp
[詳解]
1
#include "
PoseEstimator.hpp
"
2
3
#include <math.h>
4
5
namespace
{
6
7
// 地磁気外れ値棄却しきい値[rad]。屋内磁気外乱に備え残差大は棄却する。
8
constexpr
float
kMagGateRad = 1.0f;
9
// バイアス更新の低速ゲート[m/s]。走行中のエンコーダノイズ混入を防ぐ。
10
constexpr
float
kBiasGateVel = 0.05f;
11
12
}
// namespace
13
14
PoseEstimator::PoseEstimator
(
double
counts_per_rev,
double
wheel_radius,
double
wheel_base,
bool
use_ekf)
15
: vel_calc_(counts_per_rev, wheel_radius, 0.015)
16
, use_ekf_(use_ekf)
17
, wheel_base_(wheel_base)
18
{
19
odom_.setWheelBase(wheel_base);
20
}
21
22
void
PoseEstimator::applyParams
(
double
wheel_radius,
double
wheel_base) {
23
vel_calc_.setWheelRadius(wheel_radius);
24
odom_.setWheelBase(wheel_base);
25
wheel_base_ = wheel_base;
26
}
27
28
void
PoseEstimator::reset
() {
29
odom_.reset();
30
ekf_.reset();
31
cum_l_ = cum_r_ = 0;
32
cum_prev_l_ = cum_prev_r_ = 0;
33
vel_l_ = vel_r_ = 0.0;
34
w_ = 0.0f;
35
}
36
37
void
PoseEstimator::update
(int64_t dl, int64_t dr,
double
dist_l,
double
dist_r,
38
const
EstimatorImu
&imu,
double
dt_sec) {
39
cum_l_ += dl;
40
cum_r_ += dr;
41
42
// 単発スパイクは参照だけ進めて積算・速度を凍結する。
43
// calculateBothWheels は前回値を常に進めるため、次周期に跳躍を持ち越さない。
44
const
bool
glitch = (dl >
kMaxDeltaPerCycle
|| dl < -
kMaxDeltaPerCycle
||
45
dr >
kMaxDeltaPerCycle
|| dr < -
kMaxDeltaPerCycle
);
46
47
vel_calc_.setDeltaTime(dt_sec);
48
double
tmp_l = vel_l_;
49
double
tmp_r = vel_r_;
50
vel_calc_.calculateBothWheels(cum_l_, cum_r_, cum_prev_l_, cum_prev_r_, tmp_l, tmp_r);
51
if
(glitch) {
52
return
;
53
}
54
vel_l_ = tmp_l;
55
vel_r_ = tmp_r;
56
57
const
float
v_enc =
static_cast<
float
>
((vel_l_ + vel_r_) * 0.5);
58
float
w_enc = 0.0f;
59
if
(wheel_base_ > 1e-9) {
60
w_enc =
static_cast<
float
>
((vel_r_ - vel_l_) / wheel_base_);
61
}
62
63
if
(use_ekf_) {
64
// EKF有効時はpose/velocityをEKFが出すためOdometryCalculatorは温存する
65
updateEkf(imu, v_enc, w_enc,
static_cast<
float
>
(dt_sec));
66
}
else
{
67
odom_.update(dist_l, dist_r, dt_sec);
68
w_ = odom_.v_angular;
69
}
70
}
71
72
void
PoseEstimator::updateEkf(
const
EstimatorImu
&imu,
float
v_enc,
float
w_enc,
float
dt_sec) {
73
// ジャイロ不通時はエンコーダ角速度で代用する(バイアス更新は行わない)
74
const
float
gz = imu.
imu_ok
? imu.
gz
: w_enc;
75
ekf_.
predict
(gz, dt_sec);
76
ekf_.
updateVel
(v_enc);
77
if
(imu.
imu_ok
) {
78
ekf_.
updateYawRateGated
(w_enc, imu.
gz
, v_enc, kBiasGateVel);
79
// 停止検出時は v=0 拘束で加速度計を活用する(ZUPT)
80
if
(
Mirs2605Ekf::isStationary
(imu.
ax
, imu.
ay
, imu.
az
, imu.
gz
, v_enc)) {
81
ekf_.
updateZeroVel
();
82
}
83
}
84
if
(imu.
mag_ok
) {
85
const
float
yaw_mag =
Mirs2605Ekf::magYawTiltCompensated
(
86
imu.
mx
, imu.
my
, imu.
mz
, imu.
ax
, imu.
ay
, imu.
az
);
87
if
(fabsf(
Mirs2605Ekf::angleResidual
(yaw_mag, ekf_.theta())) < kMagGateRad) {
88
ekf_.updateMagTheta(yaw_mag);
89
}
90
}
91
w_ = imu.
imu_ok
? (imu.
gz
- ekf_.gyroBias()) : w_enc;
92
}
93
94
float
PoseEstimator::x
()
const
{
95
return
use_ekf_ ? ekf_.x() : odom_.x;
96
}
97
98
float
PoseEstimator::y
()
const
{
99
return
use_ekf_ ? ekf_.y() : odom_.y;
100
}
101
102
float
PoseEstimator::theta
()
const
{
103
return
use_ekf_ ? ekf_.theta() : odom_.theta;
104
}
105
106
float
PoseEstimator::v
()
const
{
107
return
use_ekf_ ? ekf_.v() : odom_.v_linear;
108
}
109
110
float
PoseEstimator::w
()
const
{
111
return
w_;
112
}
113
114
float
PoseEstimator::velL
()
const
{
115
return
static_cast<
float
>
(vel_l_);
116
}
117
118
float
PoseEstimator::velR
()
const
{
119
return
static_cast<
float
>
(vel_r_);
120
}
PoseEstimator.hpp
車輪とIMUの計測から、車輪速度と機体の位置・向きをまとめて更新する。
Mirs2605Ekf::updateVel
void updateVel(float v_enc, float r_override=-1.0f)
エンコーダ前進速度で v を補正する。
定義
Mirs2605Ekf.cpp:181
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::magYawTiltCompensated
static float magYawTiltCompensated(float mx, float my, float mz, float ax, float ay, float az)
加速度でチルト補正した地磁気ヨーを求める。
定義
Mirs2605Ekf.cpp:225
Mirs2605Ekf::predict
void predict(float gz_rad_s, float dt_sec)
予測ステップ。ジャイロで theta/x/y を伝搬する。
定義
Mirs2605Ekf.cpp:76
Mirs2605Ekf::updateZeroVel
void updateZeroVel(float r_override=-1.0f)
停止中拘束。v=0 の観測として扱う(ZUPT)。
定義
Mirs2605Ekf.cpp:212
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
PoseEstimator::y
float y() const
定義
PoseEstimator.cpp:98
PoseEstimator::update
void update(int64_t dl, int64_t dr, double dist_l, double dist_r, const EstimatorImu &imu, double dt_sec)
状態推定1ステップ。
定義
PoseEstimator.cpp:37
PoseEstimator::velL
float velL() const
定義
PoseEstimator.cpp:114
PoseEstimator::reset
void reset()
pose・速度・累積をゼロ化する(/reset_odometry 用)。
定義
PoseEstimator.cpp:28
PoseEstimator::applyParams
void applyParams(double wheel_radius, double wheel_base)
パラメータ適用(変更時のみ呼ぶこと)。
定義
PoseEstimator.cpp:22
PoseEstimator::velR
float velR() const
定義
PoseEstimator.cpp:118
PoseEstimator::PoseEstimator
PoseEstimator(double counts_per_rev, double wheel_radius, double wheel_base, bool use_ekf)
定義
PoseEstimator.cpp:14
PoseEstimator::theta
float theta() const
定義
PoseEstimator.cpp:102
PoseEstimator::v
float v() const
定義
PoseEstimator.cpp:106
PoseEstimator::kMaxDeltaPerCycle
static constexpr int64_t kMaxDeltaPerCycle
定義
PoseEstimator.hpp:32
PoseEstimator::w
float w() const
定義
PoseEstimator.cpp:110
PoseEstimator::x
float x() const
定義
PoseEstimator.cpp:94
EstimatorImu
推定器へのIMU入力(値型・FW非依存)。
定義
PoseEstimator.hpp:20
EstimatorImu::ay
float ay
定義
PoseEstimator.hpp:23
EstimatorImu::my
float my
定義
PoseEstimator.hpp:25
EstimatorImu::imu_ok
bool imu_ok
定義
PoseEstimator.hpp:21
EstimatorImu::mag_ok
bool mag_ok
定義
PoseEstimator.hpp:22
EstimatorImu::mz
float mz
定義
PoseEstimator.hpp:25
EstimatorImu::az
float az
定義
PoseEstimator.hpp:23
EstimatorImu::ax
float ax
定義
PoseEstimator.hpp:23
EstimatorImu::gz
float gz
定義
PoseEstimator.hpp:24
EstimatorImu::mx
float mx
定義
PoseEstimator.hpp:25
lib
PoseEstimator
PoseEstimator.cpp
構築:
1.18.0