mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
PoseEstimator.cpp
[詳解]
1#include "PoseEstimator.hpp"
2
3#include <math.h>
4
5namespace {
6
7// 地磁気外れ値棄却しきい値[rad]。屋内磁気外乱に備え残差大は棄却する。
8constexpr float kMagGateRad = 1.0f;
9// バイアス更新の低速ゲート[m/s]。走行中のエンコーダノイズ混入を防ぐ。
10constexpr float kBiasGateVel = 0.05f;
11
12} // namespace
13
14PoseEstimator::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
22void 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
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
37void 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 ||
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
72void 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
94float PoseEstimator::x() const {
95 return use_ekf_ ? ekf_.x() : odom_.x;
96}
97
98float PoseEstimator::y() const {
99 return use_ekf_ ? ekf_.y() : odom_.y;
100}
101
102float PoseEstimator::theta() const {
103 return use_ekf_ ? ekf_.theta() : odom_.theta;
104}
105
106float PoseEstimator::v() const {
107 return use_ekf_ ? ekf_.v() : odom_.v_linear;
108}
109
110float PoseEstimator::w() const {
111 return w_;
112}
113
114float PoseEstimator::velL() const {
115 return static_cast<float>(vel_l_);
116}
117
118float PoseEstimator::velR() const {
119 return static_cast<float>(vel_r_);
120}
車輪とIMUの計測から、車輪速度と機体の位置・向きをまとめて更新する。
void updateVel(float v_enc, float r_override=-1.0f)
エンコーダ前進速度で v を補正する。
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付近かつ角速度・車輪速度が微小。
static float magYawTiltCompensated(float mx, float my, float mz, float ax, float ay, float az)
加速度でチルト補正した地磁気ヨーを求める。
void predict(float gz_rad_s, float dt_sec)
予測ステップ。ジャイロで theta/x/y を伝搬する。
void updateZeroVel(float r_override=-1.0f)
停止中拘束。v=0 の観測として扱う(ZUPT)。
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)
低速ゲート付きバイアス更新。
void update(int64_t dl, int64_t dr, double dist_l, double dist_r, const EstimatorImu &imu, double dt_sec)
状態推定1ステップ。
void reset()
pose・速度・累積をゼロ化する(/reset_odometry 用)。
void applyParams(double wheel_radius, double wheel_base)
パラメータ適用(変更時のみ呼ぶこと)。
PoseEstimator(double counts_per_rev, double wheel_radius, double wheel_base, bool use_ekf)
static constexpr int64_t kMaxDeltaPerCycle
推定器へのIMU入力(値型・FW非依存)。