多传感器融合定位 第七章 基于滤波的融合方法
多传感器融合定位 第七章 基于滤波的融合方法
参考博客：深蓝学院-多传感器融合定位-第7章作业
本次作业：主要参考张松鹏大佬的代码，因为大佬的解析太好了，为了保留记录，以下大部分文字，均摘自大佬的原话~
代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E7%AC%AC%E4%BA%94%E7%AB%A0%20%E6%83%AF%E6%80%A7%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86%E5%8F%8A%E8%AF%AF%E5%B7%AE%E5%88%86%E6%9E%90/imu_tk
1.环境配置
1.1 protoc 版本问题
前几章使用的protoc 的版本为3.14， 第七章使用的proto版本为3.15
解决方法：需要安装新版本的proto 3.15x，按照第四章的方式生成对应的文件。
按照GeYao README中的方法，重新生成基于自己基环境protobuf的proto：
打开lidar_localization/config/scan_context文件夹，输入如下命令，生成pb文件
protoc --cpp_out=./ key_frames.proto
protoc --cpp_out=./ ring_keys.proto
protoc --cpp_out=./ scan_contexts.proto
mv key_frames.pb.cc key_frames.pb.cpp
mv ring_keys.pb.cc ring_keys.pb.cpp
mv scan_contexts.pb.cc scan_contexts.pb.cpp
分别修改生成的三个.pb.cpp文件。如下，以ring_keys.pb.cpp为例。
// Generated by the protocol buffer compiler. DO NOT EDIT!
// source: ring_keys.proto
#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
#include "ring_keys.pb.h" 替换为 #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"
#include 之后，用以上步骤生成的的.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
中的同名文件。
将.pb.cpp文件替换（注意：需要剪切，确保config文件中新生成的文件都转移到对应目录下，不能重复出现）lidar_localization/src/models/scan_context_manager中的同名文件。
1.2 缺少 fmt 库
git clone https://github.com/fmtlib/fmt
cd fmt/
mkdir build
cd build
cmake ..
make
sudo make install
编译过程中出现：error_state_kalman_filter.cpp:(.text.unlikely+0x84)：对‘fmt::v8::detail::assert_fail(char const, int, char const)’未定义的引用**
参考网址： undefined reference to `vtable for fmt::v7::format_error‘
cd catkin_ws/src/lidar_localization/cmake/sophus.cmake
list append 添加 fmt
# sophus.cmake
find_package (Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
list(APPEND ALL_TARGET_LIBRARIES ${Sophus_LIBRARIES} fmt)
1.3 glog缺少gflag的依赖
logging.cc:(.text+0x6961)：对‘google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)’未定义的引用
#解决办法： 打开glog.cmake , 末尾改为
list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES} libgflags.a libglog.a)
2.error state kalmam filter
FILE: lidar_localization/src/model/kalman_filter/error_state_kalman_filter.cpp
2.1 流程框图
2.2 代码结构
滤波算法主要包括预测(Update函数)和观测(Correct函数)两个部分:
- 预测部分接收imu数据,基于惯性解算更新名义值,基于状态方程更新误差值。
- 观测部分同时接收imu数据和定位数据,首先利用imu数据进行预测保证状态与定位数据
时间同步,然后基于观测方程计算误差值,最后利用误差值对名义值进行修正,并将误
差值清零。
2.3 初始化： Init
这里特别注意,框架是基于第一期课程,其中的旋转误差放在了导航系(n系)下,而第三期将旋转误差放在了机器人坐标系(b系)下,所以公式有所不同,特别是状态方程所用到的加速度应该是在b系下,也就是UpdateProcessEquation函数传入的linear_acc_mid应该是在b系下。所有调用到这个函数的地方都应该进行修改。
修改1：FUNCTION: ErrorStateKalmanFilter::ErrorStateKalmanFilter(const YAML::Node &node) {}
// set process equation in case of one step prediction & correction:
Eigen::Vector3d linear_acc_init(imu_data.linear_acceleration.x,
imu_data.linear_acceleration.y,
imu_data.linear_acceleration.z);
Eigen::Vector3d angular_vel_init(imu_data.angular_velocity.x,
imu_data.angular_velocity.y,
imu_data.angular_velocity.z);
// covert to navigation frame: // 把 IMU 的 velocity angular（flu系）转换到 导航系 下
linear_acc_init = linear_acc_init - accl_bias_; // body 系下
angular_vel_init = GetUnbiasedAngularVel(angular_vel_init, C_nb);
// init process equation, in case of direct correct step:
UpdateProcessEquation(linear_acc_init, angular_vel_init);
修改2：FUNCTION: ErrorStateKalmanFilter::GetVelocityDelta( )
bool ErrorStateKalmanFilter::GetVelocityDelta(
const size_t index_curr, const size_t index_prev,
const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev, double &T,
Eigen::Vector3d &velocity_delta, Eigen::Vector3d &linear_acc_mid) {
if (index_curr <= index_prev || imu_data_buff_.size() <= index_curr) {
return false;
}
const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);
T = imu_data_curr.time - imu_data_prev.time;
Eigen::Vector3d linear_acc_curr = Eigen::Vector3d(
imu_data_curr.linear_acceleration.x, imu_data_curr.linear_acceleration.y,
imu_data_curr.linear_acceleration.z);
Eigen::Vector3d a_curr = GetUnbiasedLinearAcc(linear_acc_curr, R_curr); // w系下的a_curr
Eigen::Vector3d linear_acc_prev = Eigen::Vector3d(
imu_data_prev.linear_acceleration.x, imu_data_prev.linear_acceleration.y,
imu_data_prev.linear_acceleration.z);
Eigen::Vector3d a_prev = GetUnbiasedLinearAcc(linear_acc_prev, R_prev); // w系下的a_prev
// mid-value acc can improve error state prediction accuracy:
linear_acc_mid = 0.5 * (a_curr + a_prev); // w 系下的linear_acc_mid , 用于更新pos_w 和 vel_w
velocity_delta = T * linear_acc_mid;
linear_acc_mid = 0.5 * (linear_acc_curr + linear_acc_prev) - accl_bias_; // b 系下的linear_acc_mid
return true;
}
这里有个之前一直忽略的点，在此mark下笔记：
inline Eigen::Vector3d ErrorStateKalmanFilter::GetUnbiasedAngularVel(
const Eigen::Vector3d &angular_vel, const Eigen::Matrix3d &R) {
return angular_vel - gyro_bias_;
}
inline Eigen::Vector3d
ErrorStateKalmanFilter::GetUnbiasedLinearAcc(const Eigen::Vector3d &linear_acc,
const Eigen::Matrix3d &R) {
return R * (linear_acc - accl_bias_) - g_;
}
两个函数，区别在于，惯性解算时
更新四元数时，只需要得到相对旋转，在body系下就可以得到相对旋转，所以不需要乘以R。
更新位置时，需要把速度转换到n系下，所以需要乘以R。
在init filter 初始化滤波器时，
angular_vel_init 、linear_acc_init 都是b 系下的
// 可以调用 GetUnbiasedAngularVel ，因为角速度仍然时在b系下的
angular_vel_init = GetUnbiasedAngularVel(angular_vel_init, C_nb);
// 不可以调用 GetUnbiasedLinearAcc ，因为调用后加速度换左乘R，变换到n系下的
linear_acc_init = linear_acc_init - accl_bias_; // body 系下
2.4 预测: Update
包含： 1.更新名义值UpdateOdomEstimation
2.更新误差值UpdateErrorEstimation
bool ErrorStateKalmanFilter::Update(const IMUData &imu_data) { // 更新
//
// TODO: understand ESKF update workflow
//
// update IMU buff:
if (time_ < imu_data.time) {
// update IMU odometry:
Eigen::Vector3d linear_acc_mid;
Eigen::Vector3d angular_vel_mid;
imu_data_buff_.push_back(imu_data);
UpdateOdomEstimation(linear_acc_mid, angular_vel_mid); // 更新名义值 ， 惯性解算
imu_data_buff_.pop_front();
// update error estimation:
double T = imu_data.time - time_; // 滤波周期
UpdateErrorEstimation(T, linear_acc_mid, angular_vel_mid); // 更新误差估计
// move forward:
time_ = imu_data.time;
return true;
}
return false;
}
2.4.1 更新名义值 UpdateOdomEstimation
这部分是上一章惯性导航解算的内容;
void ErrorStateKalmanFilter::UpdateOdomEstimation( // 更新名义值
Eigen::Vector3d &linear_acc_mid, Eigen::Vector3d &angular_vel_mid) {
//
// TODO: this is one possible solution to previous chapter, IMU Navigation,
// assignment
//
// get deltas:
size_t index_curr_ = 1;
size_t index_prev_ = 0;
Eigen::Vector3d angular_delta = Eigen::Vector3d::Zero();
GetAngularDelta(index_curr_, index_prev_, angular_delta, angular_vel_mid); // 获取等效旋转矢量, 保存角速度中值
// update orientation:
Eigen::Matrix3d R_curr_ = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R_prev_ = Eigen::Matrix3d::Identity();
UpdateOrientation(angular_delta, R_curr_, R_prev_); // 更新四元数
// get velocity delta:
double delta_t_;
Eigen::Vector3d velocity_delta_;
GetVelocityDelta(index_curr_, index_prev_, R_curr_, R_prev_, delta_t_, velocity_delta_, linear_acc_mid); // 获取速度差值， 保存线加速度中值
// save mid-value unbiased linear acc for error-state update:
// update position:
UpdatePosition(delta_t_, velocity_delta_);
}
这里需要注意,GetVelocityDelta函数中的linear_acc_mid应该返回在b系下的测量值
2.4.2 更新误差值UpdateErrorEstimation
首先调用UpdateProcessEquation计算状态方程中的F和B,该函数进一步调用SetProcessEquation函数
UpdateErrorEstimation()
void ErrorStateKalmanFilter::UpdateErrorEstimation( // 更新误差值
const double &T, const Eigen::Vector3d &linear_acc_mid,
const Eigen::Vector3d &angular_vel_mid) {
static MatrixF F_1st;
static MatrixF F_2nd;
// TODO: update process equation: // 更新状态方程
UpdateProcessEquation(linear_acc_mid , angular_vel_mid);
// TODO: get discretized process equations: // 非线性化
F_1st = F_ * T; // T kalman 周期
MatrixF F = MatrixF::Identity() + F_1st;
MatrixB B = MatrixB::Zero();
B.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) = B_.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) * T;
B.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) = B_.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) *T;
if(COV.PROCESS.BIAS_FLAG){
B.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel) = B_.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel)* sqrt(T);
B.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro) = B_.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro)* sqrt(T);
}
// TODO: perform Kalman prediction
X_ = F * X_;
P_ = F * P_ * F.transpose() + B * Q_ * B.transpose(); // 只有方差进行了计算
}
UpdateProcessEquation()
void ErrorStateKalmanFilter::UpdateProcessEquation(
const Eigen::Vector3d &linear_acc_mid,
const Eigen::Vector3d &angular_vel_mid) {
// set linearization point:
Eigen::Matrix3d C_nb = pose_.block<3, 3>(0, 0); // n2b 转换矩阵
Eigen::Vector3d f_b = linear_acc_mid + g_; // 加速度
Eigen::Vector3d w_b = angular_vel_mid; // 角速度
// set process equation:
SetProcessEquation(C_nb, f_b, w_b);
}
SetProcessEquation()
void ErrorStateKalmanFilter::SetProcessEquation(const Eigen::Matrix3d &C_nb, // 更新状态方程 F矩阵
const Eigen::Vector3d &f_b,
const Eigen::Vector3d &w_b) {
// TODO: set process / system equation:
// a. set process equation for delta vel:
F_.setZero();
F_.block<3, 3>(kIndexErrorPos, kIndexErrorVel) = Eigen::Matrix3d::Identity();
F_.block<3, 3>(kIndexErrorVel, kIndexErrorOri) = - C_nb * Sophus::SO3d::hat(f_b).matrix();
F_.block<3, 3>(kIndexErrorVel, kIndexErrorAccel) = -C_nb;
F_.block<3, 3>(kIndexErrorOri, kIndexErrorOri) = - Sophus::SO3d::hat(w_b).matrix();
F_.block<3, 3>(kIndexErrorOri, kIndexErrorGyro) = - Eigen::Matrix3d::Identity();
// b. set process equation for delta ori:
B_.setZero();
B_.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) = C_nb;
B_.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) = Eigen::Matrix3d::Identity();
if(COV.PROCESS.BIAS_FLAG){ // 判断是否考虑随机游走
B_.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel) = Eigen::Matrix3d::Identity();
B_.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro) = Eigen::Matrix3d::Identity();
}
}
2.5 观测 Correct
bool ErrorStateKalmanFilter::Correct(const IMUData &imu_data, // 修正
const MeasurementType &measurement_type,
const Measurement &measurement) {
static Measurement measurement_;
// get time delta:
double time_delta = measurement.time - time_;
if (time_delta > -0.05) { // 时间对齐
// perform Kalman prediction:
if (time_ < measurement.time) {
Update(imu_data);
}
// get observation in navigation frame:
measurement_ = measurement;
measurement_.T_nb = init_pose_ * measurement_.T_nb;
// correct error estimation:
CorrectErrorEstimation(measurement_type, measurement_);
// eliminate error:
EliminateError(); // 更新名义值
// reset error state:
ResetState(); // 清零误差值，方差保留
return true;
}
2.5.1 计算误差值 CorrectErrorEstimation
a. 首先调用CorrectErrorEstimationPose 计算 Y, G, K。
void ErrorStateKalmanFilter::CorrectErrorEstimationPose( // 计算 Y ，G ，K
const Eigen::Matrix4d &T_nb, Eigen::VectorXd &Y, Eigen::MatrixXd &G,
Eigen::MatrixXd &K) {
//
// TODO: set measurement: 计算观测 delta pos 、 delta ori
//
Eigen::Vector3d dp = pose_.block<3, 1>(0, 3) - T_nb.block<3, 1>(0, 3);
Eigen::Matrix3d dR = T_nb.block<3, 3>(0, 0).transpose() * pose_.block<3, 3>(0, 0) ;
// TODO: set measurement equation:
Eigen::Vector3d dtheta = Sophus::SO3d::vee(dR - Eigen::Matrix3d::Identity() );
YPose_.block<3, 1>(0, 0) = dp; // delta position
YPose_.block<3, 1>(3, 0) = dtheta; // 失准角
Y = YPose_;
// set measurement G
GPose_.setZero();
GPose_.block<3, 3>(0, kIndexErrorPos) = Eigen::Matrix3d::Identity();
GPose_.block<3 ,3>(3, kIndexErrorOri) = Eigen::Matrix3d::Identity();
G = GPose_;
// set measurement C
CPose_.setZero();
CPose_.block<3, 3>(0,kIndexNoiseAccel) = Eigen::Matrix3d::Identity();
CPose_.block<3, 3>(3,kIndexNoiseGyro) = Eigen::Matrix3d::Identity();
Eigen::MatrixXd C = CPose_;
// TODO: set Kalman gain:
Eigen::MatrixXd R = RPose_; // 观测噪声
K = P_ * G.transpose() * ( G * P_ * G.transpose( ) + C * RPose_* C.transpose() ).inverse() ;
}
b. 更新后验，计算滤波的第4，5个公式
void ErrorStateKalmanFilter::CorrectErrorEstimation(
const MeasurementType &measurement_type, const Measurement &measurement) {
//
// TODO: understand ESKF correct workflow
//
Eigen::VectorXd Y;
Eigen::MatrixXd G, K;
switch (measurement_type) {
case MeasurementType::POSE:
CorrectErrorEstimationPose(measurement.T_nb, Y, G, K);
break;
default:
break;
}
// TODO: perform Kalman correct:
P_ = (MatrixP::Identity() - K*G) * P_ ; // 后验方差
X_ = X_ + K * (Y - G*X_); // 更新后的状态量
}
2.5.2 修正名义值 EliminateError
修正平移,速度,旋转和零偏,修正方式是预测值减去误差值。
void ErrorStateKalmanFilter::EliminateError(void) {
// 误差状态量 X_ : 15*1
// TODO: correct state estimation using the state of ESKF
//
// a. position:
// do it!
pose_.block<3, 1>(0, 3) -= X_.block<3, 1>(kIndexErrorPos, 0 ); // 减去error
// b. velocity:
// do it!
vel_ -= X_.block<3,1>(kIndexErrorVel, 0 );
// c. orientation:
// do it!
Eigen::Matrix3d dtheta_cross = Sophus::SO3d::hat(X_.block<3,1>(kIndexErrorOri, 0)); // 失准角的反对称矩阵
pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * (Eigen::Matrix3d::Identity() - dtheta_cross);
Eigen::Quaterniond q_tmp(pose_.block<3, 3>(0, 0) );
q_tmp.normalize(); // 为了保证旋转矩阵是正定的
pose_.block<3, 3>(0, 0) = q_tmp.toRotationMatrix();
// d. gyro bias:
if (IsCovStable(kIndexErrorGyro)) {
gyro_bias_ -= X_.block<3, 1>(kIndexErrorGyro, 0); // 判断gyro_bias_error是否可观
}
// e. accel bias:
if (IsCovStable(kIndexErrorAccel)) {
accl_bias_ -= X_.block<3, 1>(kIndexErrorAccel, 0); // 判断accel_bias_error是否可观
}
}
2.5.3 清零误差值 ResetState
void ErrorStateKalmanFilter::ResetState(void) {
// reset current state:
X_ = VectorX::Zero();
}
Running
# build:
catkin_make
# set up session:
source devel/setup.bash
# launch:
roslaunch lidar_localization kitti_localization.launch
# play ROS bag, lidar-only KITTI:
rosbag play kitti_lidar_only_2011_10_03_drive_0027_synced.bag
# evo estimate
3.不考虑随机游走
3.1 公式推导
状态方程：
δ x ˙ = F t δ x + B t w \delta\dot{x}= F_t\delta{x} + B_tw δx˙=Ftδx+Btw
状态量：
δ p ˙ = δ v δ v ˙ = − R t [ a t − b a t ] × δ θ + R t ( n a − δ b a ) δ θ ˙ = − [ ω t − b ω t ] × δ θ + n ω − δ b ω δ b ˙ a = 0 δ b ˙ w = 0 \begin{array}{ll} \delta \dot{p} = \delta \boldsymbol{v} \\ \delta \dot{\boldsymbol{v}} =-\boldsymbol{R}_{t}\left[\boldsymbol{a}_{t}-\boldsymbol{b}_{a_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{R}_{t}\left(\boldsymbol{n}_{a}-\delta \boldsymbol{b}_{a}\right) \\ \delta \dot{\boldsymbol{\theta}} =-\left[\boldsymbol{\omega}_{t}-\boldsymbol{b}_{\omega_{t}}\right]_{\times} \delta \boldsymbol{\theta}+\boldsymbol{n}_{\omega}-\delta \boldsymbol{b}_{\omega} \\ \delta \dot{\boldsymbol{b}}_{a}=0 \\ \delta \dot{\boldsymbol{b}}_{w}=0 \\ \end{array} δp˙=δvδv˙=−Rt[at−bat]×δθ+Rt(na−δba)δθ˙=−[ωt−bωt]×δθ+nω−δbωδb˙a=0δb˙w=0
过程噪声部分：(线性kalman)
KaTeX parse error: Expected 'EOF', got '&' at position 126: …array}\right] &̲&&& \boldsymbol…
过程噪声部分：(非线性kalman)
KaTeX parse error: Expected 'EOF', got '&' at position 126: …array}\right] &̲&&& \boldsymbol…
3.2 修改部分
在 kitti_filtering.yaml 中添加bias_flag 选项，以选择是否考虑使用随机游走
FILE: /catkin_ws/src/lidar_localization/config/filtering/kitti_filtering.yaml
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process:
gyro: 1.0e-4
accel: 2.5e-3
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: true
measurement:
pose:
pos: 1.0e-4
ori: 1.0e-4
pos: 1.0e-4
vel: 2.5e-3
Q矩阵
FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp
FUNCTION: ErrorStateKalmanFilter::ErrorStateKalmanFilter( )
// c. process noise: 过程噪声
Q_.block<3, 3>(kIndexNoiseAccel, kIndexNoiseAccel) = COV.PROCESS.ACCEL * Eigen::Matrix3d::Identity();
Q_.block<3, 3>(kIndexNoiseGyro, kIndexNoiseGyro) = COV.PROCESS.GYRO * Eigen::Matrix3d::Identity();
if (COV.PROCESS.BIAS_FLAG ){
Q_.block<3, 3>(kIndexNoiseBiasAccel, kIndexNoiseBiasAccel) = COV.PROCESS.BIAS_ACCEL * Eigen::Matrix3d::Identity();
Q_.block<3, 3>(kIndexNoiseBiasGyro, kIndexNoiseBiasGyro) = COV.PROCESS.BIAS_GYRO * Eigen::Matrix3d::Identity();
}
线性kalman B矩阵
FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp
FUNCTION: ErrorStateKalmanFilter::SetProcessEquation( )
// b. set process equation for delta ori:
B_.setZero();
B_.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) = C_nb;
B_.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) = Eigen::Matrix3d::Identity();
if(COV.PROCESS.BIAS_FLAG){ // 判断是否考虑随机游走
B_.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel) = Eigen::Matrix3d::Identity();
B_.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro) = Eigen::Matrix3d::Identity();
}
非线性kalman B矩阵
FILE: catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp
FUNCTION: ErrorStateKalmanFilter::UpdateErrorEstimation( )
MatrixB B = MatrixB::Zero();
B.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) = B_.block<3, 3>(kIndexErrorVel, kIndexNoiseGyro) * T;
B.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) = B_.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) *T;
if(COV.PROCESS.BIAS_FLAG){
B.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel) = B_.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel)* sqrt(T);
B.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro) = B_.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro)* sqrt(T);
}
3.3 结果
| filter with bias random walk | filter without bias random walk |
|---|---|
 |
 |
 |
 |
| max 1.521893 mean 0.901894 median 0.899981 min 0.054675 rmse 0.919171 sse 3695.484911 std 0.177375 |
max 1.525035 mean 0.318024 median 0.251188 min 0.021889 rmse 0.381344 sse 638.991278 std 0.210439 |
3.4 结果分析
从上述实验中，得知，有没有考虑随机游走影响并不是特别大，也可能是kitti数据集groundtruth 本身存在数据的问题。
4.调节ESKF Q、R参数，并进行数据对比
4.1 evo 评估指令
# set up session:
source devel/setup.bash
# save odometry:
rosservice call /save_odometry "{}"
# run evo evaluation:
# a. laser 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt laser.txt -r full --plot --plot_mode xy --save_results ./laser.zip
# b. fused 输出评估结果，并以zip的格式存储:
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy --save_results ./fused.zip
#c. 比较 laser fused 一并比较评估
evo_res *.zip -p
4.2 eskf 调参手段
本调试方法，得益于梓杰大佬的点拨，仅供参考~
主要调节 过程噪声 Q和观测噪声 R, 过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。
KaTeX parse error: Expected 'EOF', got '&' at position 176: …{array}\right] &̲&&&\quad \bolds…
可通过evo_res 指令 对比观察 laser_odom 和 fused_odom 的 APE曲线。若两者的曲线大致一致，且误差较大，则证明，滤波后的轨迹大致和观测一样 。说明观测噪声®给的小了，导致滤波后，融合的轨迹对观测的置信度更高，所以出现滤波后的轨迹和观测更像，这时应该适当把观测噪声® 加大，过程噪声(Q) 减少。同理反向调节。
rosservice call /save_odometry "{}"
evo_ape kitti ground_truth.txt laser.txt -r full --plot --plot_mode xy --save_results ./laser.zip
evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy --save_results ./fused.zip
evo_res *.zip -p
for example : 以 parametr3 为例子
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-5
accel: 2.5e-4
bias_accel: 2.5e-4
bias_gyro: 1.0e-5
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-2
ori: 1.0e-2
pos: 1.0e-4
vel: 2.5e-3
结果： 从下表可知，fused 结果误差比laser的误差要大，证明对现在对预测(IMU)的置信度较高，观测(LIDAR)的置信度较低。
调整： 提高观测的置信度，降低预测的置信度；相应地：减少观测噪声® , 增大过程噪声(Q)
APE w.r.t. full transformation (unit-less)
(not aligned)
max mean median min rmse sse std
fused.txt 1.59676 0.90307 0.902652 0.0591278 0.922722 3725.8 0.189423
laser.txt 1.50034 0.901495 0.893804 0.161967 0.916559 3676.19 0.165489
调整后 ： parameter 2为例子
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q 增大
gyro: 1.0e-4
accel: 2.5e-3
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: true
measurement: #观测噪声 R 减小
pose:
pos: 1.0e-4
ori: 1.0e-4
pos: 1.0e-4
vel: 2.5e-3
结果：调整过后，fused 的轨迹误差降低，观测的置信度更高
APE w.r.t. full transformation (unit-less)
(not aligned)
max mean median min rmse sse std
fused.txt 1.52392 0.901544 0.898788 0.054487 0.918868 3703.18 0.177588
laser.txt 1.50034 0.90192 0.89412 0.161967 0.916964 3687.85 0.1654
4.3 不同参数结果对比
parameter 1
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-5
accel: 2.5e-4
bias_accel: 2.5e-4
bias_gyro: 1.0e-5
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-3
ori: 1.0e-3
pos: 1.0e-4
vel: 2.5e-3
evo estimate
| Lidar Only | IMU-Lidar Fusion |
|---|---|
 |
 |
 |
 |
| max 1.500344 mean 0.901495 median 0.893804 min 0.161967 rmse 0.916559 sse 3676.188909 std 0.165489 |
max 1.542837 mean 0.902142 median 0.899435 min 0.058398 rmse 0.920858 sse 3714.999803 std 0.184716 |
parameter 2
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-4
accel: 2.5e-3
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-4
ori: 1.0e-4
pos: 1.0e-4
vel: 2.5e-3
evo estimate
| Lidar Only | IMU-Lidar Fusion |
|---|---|
 |
 |
 |
 |
| max 1.500344 mean 0.901920 median 0.894120 min 0.161967 rmse 0.916964 sse 3687.845753 std 0.165414 |
max 1.523916 mean 0.901544 median 0.898788 min 0.054487 rmse 0.918868 sse 3703.181677 std 0.177588 |
parameter 3
过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-5
accel: 2.5e-4
bias_accel: 2.5e-4
bias_gyro: 1.0e-5
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-2
ori: 1.0e-2
pos: 1.0e-4
vel: 2.5e-3
evo estimate
| Lidar Only | IMU-Lidar Fusion |
|---|---|
 |
 |
 |
 |
| max 1.500344 mean 0.901495 median 0.893804 min 0.161967 rmse 0.916559 sse 3676.188909 std 0.165489 |
max 1.596762 mean 0.903070 median 0.902652 min 0.059128 rmse 0.922722 sse 3725.795851 std 0.189423 |
parameter 4
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-4
accel: 2.5e-3
bias_accel: 2.5e-3
bias_gyro: 1.0e-4
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-5
ori: 1.0e-5
pos: 1.0e-4
vel: 2.5e-3
evo estimate
| Lidar Only | IMU-Lidar Fusion |
|---|---|
 |
 |
 |
 |
| max 1.136680 mean 0.230984 median 0.163519 min 0.017465 rmse 0.289158 sse 367.057912 std 0.173951 |
max 1.166536 mean 0.244853 median 0.187717 min 0.010400 rmse 0.298805 sse 391.959520 std 0.171264 |
parameter 5
过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高
covariance:
prior:
pos: 1.0e-6
vel: 1.0e-6
ori: 1.0e-6
epsilon: 1.0e-6
delta: 1.0e-6
process: #过程噪声 Q
gyro: 1.0e-6
accel: 2.5e-6
bias_accel: 2.5e-6
bias_gyro: 1.0e-6
bias_flag: true
measurement: #观测噪声 R
pose:
pos: 1.0e-2
ori: 1.0e-2
pos: 1.0e-4
vel: 2.5e-3
evo estimate
| Lidar Only | IMU-Lidar Fusion |
|---|---|
 |
 |
 |
 |
| max 1.136680 mean 0.230928 median 0.163564 min 0.017465 rmse 0.289055 sse 367.297769 std 0.173854 |
max 1.596545 mean 0.317060 median 0.259280 min 0.029770 rmse 0.375757 sse 620.687408 std 0.201660 |
如下图 evo_res 对比可得知，过程噪声调小，观测噪声调大，理论现象：IMU惯性解算的置信度更高， fused的误差结果和曲线有别于odom，现象正确。
max mean median min rmse sse std
fused.txt 1.59655 0.31706 0.25928 0.0297698 0.375757 620.687 0.20166
laser.txt 1.13668 0.230928 0.163564 0.0174648 0.289055 367.298 0.173854
5. Gnss 定位原点初始化 bug 修复
现象描述：
1.多次运行第七章课程框架中kalman filter localization 时，结束后，laser_odom 的evo 精度评估数据都不太一样，ape mean 从 0.2 到 4，均有变化。
解决：
FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp
初始化滤波部分，使用的是scancontext，可能是sc 初始化出现问题，导致每次数据都有差别，所以打算换成“第四章”的gnss初始化。
bool KITTIFilteringFlow::InitLocalization(void) {
// ego vehicle velocity in body frame:
Eigen::Vector3f init_vel = current_pos_vel_data_.vel;
first try to init using scan context query:
if (filtering_ptr_->Init(current_cloud_data_, init_vel,
current_imu_synced_data_)) {
// // prompt:
LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
}
return true;
}
修改如下：
filtering_ptr_ -> Init()
在 FILE: catkin_ws/src/lidar_localization/src/filtering/kitti_filtering.cpp 框架中，已定义好 使用 scancontext 和 gnss 两种init pose的方式，无需我们再写
/* scan contexxt 初始化 pose ， 输入 point cloud_*/
bool KITTIFiltering::Init(const CloudData &init_scan,
const Eigen::Vector3f &init_vel,
const IMUData &init_imu_data) {
if (SetInitScan(init_scan)) {
current_vel_ = init_vel;
kalman_filter_ptr_->Init(current_vel_.cast<double>(), init_imu_data);
return true;
}
return false;
}
/* gnss 初始化 pose ， 输入 martix4d current_gnss_pose_*/
bool KITTIFiltering::Init(const Eigen::Matrix4f &init_pose,
const Eigen::Vector3f &init_vel,
const IMUData &init_imu_data) {
if (SetInitGNSS(init_pose)) {
current_vel_ = init_vel;
kalman_filter_ptr_->Init(current_vel_.cast<double>(), init_imu_data);
return true;
}
return false;
}
修改 1 ： InitLocalization() 调用 Init() 方法
FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp
bool KITTIFilteringFlow::InitLocalization(void) {
// ego vehicle velocity in body frame:
Eigen::Vector3f init_vel = current_pos_vel_data_.vel;
// first try to init using scan context query:
// if (filtering_ptr_->Init(current_cloud_data_, init_vel,
// current_imu_synced_data_)) {
// // prompt:
// LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
// }
// first try to init using gnss init:
if (filtering_ptr_->Init(current_gnss_data_.pose, init_vel,
current_imu_synced_data_)){
// prompt:
LOG(INFO) << "Gnss Localization Init Succeeded." << std::endl;
}
return true;
}
修改2 ： 按照第四章gnss 初始化的部分修改, 初始化经纬高
通过记录获得建图时gnss 原点坐标为：
latitude = 48.9825452359;
longitude = 8.39036610005;
altitude = 116.382141113;
将建图原点的"经纬高" 转换为到导航系(ENU系)下的原点
FILE: lidar_localization/src/sensor_data/gnss_data.cpp
void GNSSData::InitOriginPosition() {
geo_converter.Reset(48.982658, 8.390455, 116.396412); // 设置原点
origin_longitude = longitude;
origin_latitude = latitude;
origin_altitude = altitude;
origin_position_inited = true;
}
修改3 ： 修复 current_gnss_data_ 没有更新问题
现象：按照如上方法，修改后，运行程序，初始化的gnss pose，发现定位有问题，通过打印 current_gnss_data_.pose，发现无论数据集在哪个地方开始播放，init gnss pose的值都是一个 4x4 的 单位阵，明显不正确。
问题所在： current_gnss_data_.pose 在本章节的框架中，只在save_odometry 保存groundtruth.txt 数据集被赋值，在其他地方没有被更新。
原框架中只在save_odometry 中更新 current_gnss_data_.pose
bool KITTIFilteringFlow::SaveOdometry(void) {
if (0 == trajectory.N) {
return false;
}
// init output files:
std::ofstream fused_odom_ofs;
std::ofstream laser_odom_ofs;
std::ofstream ref_odom_ofs;
if (!FileManager::CreateFile(fused_odom_ofs,
WORK_SPACE_PATH +
"/slam_data/trajectory/fused.txt") ||
!FileManager::CreateFile(laser_odom_ofs,
WORK_SPACE_PATH +
"/slam_data/trajectory/laser.txt") ||
!FileManager::CreateFile(ref_odom_ofs,
WORK_SPACE_PATH +
"/slam_data/trajectory/ground_truth.txt")) {
return false;
}
// write outputs:
for (size_t i = 0; i < trajectory.N; ++i) {
// sync ref pose with gnss measurement:
while (!gnss_data_buff_.empty() &&
(gnss_data_buff_.front().time - trajectory.time_.at(i) <= -0.05)) {
gnss_data_buff_.pop_front();
}
if (gnss_data_buff_.empty()) {
break;
}
current_gnss_data_ = gnss_data_buff_.front();
const Eigen::Vector3f &position_ref =
current_gnss_data_.pose.block<3, 1>(0, 3);
const Eigen::Vector3f &position_lidar =
trajectory.lidar_.at(i).block<3, 1>(0, 3);
if ((position_ref - position_lidar).norm() > 3.0) {
continue;
}
SavePose(trajectory.fused_.at(i), fused_odom_ofs);
SavePose(trajectory.lidar_.at(i), laser_odom_ofs);
SavePose(current_gnss_data_.pose, ref_odom_ofs);
}
return true;
}
解决
在 InitLocalization() 函数中，更新current_gnss_data_
注意：这里有个地方需要注意的是，GNSS数据一般前几帧都是不准确，所以我们舍弃前三帧, 取第四帧, 需要对 SetInitGNSS 函数进行修改一下，修改为使用第四帧的gnss数据进行gnss pose 初始化。
FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering_flow.cpp
bool KITTIFilteringFlow::InitLocalization(void) {
// ego vehicle velocity in body frame:
Eigen::Vector3f init_vel = current_pos_vel_data_.vel;
// first try to init using scan context query:
// if (filtering_ptr_->Init(current_cloud_data_, init_vel,
// current_imu_synced_data_)) {
// // prompt:
// LOG(INFO) << "Scan Context Localization Init Succeeded." << std::endl;
// }
// first try to init using gnss init:
static int gnss_count = 0;
if(!(gnss_count >3)){
current_gnss_data_ = gnss_data_buff_.at(gnss_count); // 舍弃GNSS的第三帧数据
// std::cout << " gnss_data_buff_ " << gnss_count << " " << current_gnss_data_.pose << std::endl;
}
gnss_count ++ ;
if (filtering_ptr_->Init(current_gnss_data_.pose, init_vel,
current_imu_synced_data_)){
// prompt:
LOG(INFO) << "Gnss Localization Init Succeeded." << std::endl;
}
return true;
}
FILE : catkin_ws/src/lidar_localization/src/filtering/kitti_filtering.cpp
bool KITTIFiltering::SetInitGNSS(const Eigen::Matrix4f &gnss_pose) {
static int gnss_cnt = 0;
current_gnss_pose_ = gnss_pose;
// if (gnss_cnt == 0) { // 一般gnss 数据，前几帧都不准，所以取第三帧
// SetInitPose(gnss_pose);
// } else if (gnss_cnt > 3) {
// has_inited_ = true;
// }
if (gnss_cnt > 3) {
has_inited_ = true;
}
SetInitPose(gnss_pose);
gnss_cnt++;
return true;
}
结果：
1.能够成功实现gnss 初始化，并在地图任意一点启动，初始化成功。
2.经过6次，同一套kalman 参数的实验，得出的evo 结果大致一样，可以认为修复了 每次启动evo评估结果不同的问题。
六次同一参数 evo 评估结果 ，大致相等
 |
|---|
 |
 |
 |
 |
 |
edited by kaho 2021.10.12