mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
PoseEstimator.hpp
[詳解]
1
9#pragma once
10
11#include <stdint.h>
12
13#include "Mirs2605Ekf.hpp"
16
21 bool imu_ok = false;
22 bool mag_ok = false;
23 float ax = 0.0f, ay = 0.0f, az = 0.0f;
24 float gz = 0.0f;
25 float mx = 0.0f, my = 0.0f, mz = 0.0f;
26};
27
29public:
30 // 単発スパイク棄却しきい値 [counts/周期]。1周期の真の移動は最大でも
31 // 数百カウント(0.8m/s・15msで約73)のため、超過分は積算・速度更新しない。
32 static constexpr int64_t kMaxDeltaPerCycle = 2048;
33
34 PoseEstimator(double counts_per_rev, double wheel_radius, double wheel_base, bool use_ekf);
35
37 void applyParams(double wheel_radius, double wheel_base);
38
40 void reset();
41
51 void update(int64_t dl, int64_t dr, double dist_l, double dist_r,
52 const EstimatorImu &imu, double dt_sec);
53
54 // 融合結果(EKF無効時はエンコーダオドメトリ)
55 float x() const;
56 float y() const;
57 float theta() const;
58 float v() const; // 前進速度[m/s](/odom twist用)
59 float w() const; // 角速度[rad/s](同上)
60 // PIDフィードバック用車輪速度[m/s]
61 float velL() const;
62 float velR() const;
63
64private:
65 void updateEkf(const EstimatorImu &imu, float v_enc, float w_enc, float dt_sec);
66
68 Mirs2605Ekf ekf_;
69 VelocityCalculator vel_calc_;
70 bool use_ekf_;
71 double wheel_base_;
72 int64_t cum_l_ = 0;
73 int64_t cum_r_ = 0;
74 int64_t cum_prev_l_ = 0;
75 int64_t cum_prev_r_ = 0;
76 double vel_l_ = 0.0;
77 double vel_r_ = 0.0;
78 float w_ = 0.0f;
79};
車輪とIMUの値から、計測のばらつきを考慮して機体の位置・向きを推定する。
エンコーダの増分を左右車輪の移動距離に換算する。
車輪の移動距離と経過時間から速度を求める。
差動二輪オドメトリ計算器。
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
エンコーダカウント→車輪速度[m/s]の時間微分計算。
推定器へのIMU入力(値型・FW非依存)。