mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
PoseEstimator.hpp
[詳解]
1
9
#pragma once
10
11
#include <stdint.h>
12
13
#include "
Mirs2605Ekf.hpp
"
14
#include "
OdometryCalculator.hpp
"
15
#include "
VelocityCalculator.hpp
"
16
20
struct
EstimatorImu
{
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
28
class
PoseEstimator
{
29
public
:
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
64
private
:
65
void
updateEkf(
const
EstimatorImu
&imu,
float
v_enc,
float
w_enc,
float
dt_sec);
66
67
OdometryCalculator
odom_;
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
};
Mirs2605Ekf.hpp
車輪とIMUの値から、計測のばらつきを考慮して機体の位置・向きを推定する。
OdometryCalculator.hpp
エンコーダの増分を左右車輪の移動距離に換算する。
VelocityCalculator.hpp
車輪の移動距離と経過時間から速度を求める。
Mirs2605Ekf
定義
Mirs2605Ekf.hpp:25
OdometryCalculator
差動二輪オドメトリ計算器。
定義
OdometryCalculator.hpp:11
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
VelocityCalculator
エンコーダカウント→車輪速度[m/s]の時間微分計算。
定義
VelocityCalculator.hpp:14
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.hpp
構築:
1.18.0