eskf 不太懂的看看高博的《视觉SLAM14讲》和和高博的简明ESKF
说实话,我看了4-5遍了,只能浅浅领悟,我感觉很多东西被公式化了,不要看到一大堆公式就被吓倒,这些公式一一简化就是+ - * /。
其实如果能有一本数学的科普书,或者视频教程就好了。把一些什么协方差,滤波,偏导,指数,反对称矩阵,旋转矩阵,扰动,旋转向量,四元数,以直观的形式表达。
所以到现在我还是只能浅浅理解。阶段性记录,与君共享。
ESKF与KF 、EKF有什么不同?一个是过滤当前值
过滤 你可以理解成平均,1号评委打分9.7,2号评委打分9.5,那选手的得分过滤后是9.6=(9.7+9.5)/ 2,这里可以理解为KF和EKF的方式
ESKF与KF、EKF有什么不同呢?还是用评委打分的方式,1号评委打分为9.7(基础得分9.6+印象分0.1),2号评委打分为9.5(基础得分9.6+印象分-0.1),那使用ESKF如何计算选手的最终得分呢?
最终的分:9.6+(0.1+ (-0.1))/2 = 9.6
有的同学说如果2个评委的基础得分不一样怎么办?那9.6是不是也要平均一下?
这个就是算法,你想怎么处理都可以,公式是死的,人是活的,也许人家的公式也要优化与改进。
ESKF只是一个数据融合的方式,可能比KF、EKF稳定一些吧。
//
// Created by xiang on 2021/11/11.
//
#include "ch3/eskf.hpp"
#include "ch3/static_imu_init.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"
#include "utm_convert.h"
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <fstream>
#include <iomanip>
DEFINE_string(txt_path, "/home/peak/slam_in_autonomous_driving-master/data/ch3/10.txt", "数据文件路径");
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角(角度)");
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
DEFINE_bool(with_ui, true, "是否显示图形界面");
DEFINE_bool(with_odom, false, "是否加入轮速计信息");
/**
* 本程序演示使用RTK+IMU进行组合导航
*/
int main(int argc, char** argv) {
//谷歌的日志库
google::InitGoogleLogging(argv[0]);
FLAGS_stderrthreshold = google::INFO;
FLAGS_colorlogtostderr = true;
google::ParseCommandLineFlags(&argc, &argv, true);
if (fLS::FLAGS_txt_path.empty()) {
return -1;
}
// 初始化器
sad::StaticIMUInit imu_init; // 使用默认配置
//eskf类
sad::ESKFD eskf;
sad::TxtIO io(FLAGS_txt_path);
//GPS显示的位置
Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
//这是一个函数,用来保存数据,怎么用?看下面,
//【】是什么意思,说实话我又忘记了,还有【=】【&】,
// 如果真的不习惯这种用法,把他当成函数指针。【】【=】【&】不去管他(换汤不换药,也许以后用多了就习惯了,)
auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
auto save_quat = [](std::ofstream& fout, const Quatd& q) {
fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
};
auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
save_vec3(fout, save_state.p_);
save_quat(fout, save_state.R_.unit_quaternion());
save_vec3(fout, save_state.v_);
save_vec3(fout, save_state.bg_);
save_vec3(fout, save_state.ba_);
fout << std::endl;
};
std::ofstream fout("./data/ch3/gins.txt");
bool imu_inited = false, gnss_inited = false;
//共享指针,C++要自己回收内存的诞生产品,感觉做的还有漏洞,所以又诞生了另一个指针类型weak_ptr;把它当成一个指针就好了。
std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
if (FLAGS_with_ui) {
ui = std::make_shared<sad::ui::PangolinWindow>();
ui->Init();
}
/// 设置各类回调函数
bool first_gnss_set = false;
Vec3d origin = Vec3d::Zero();
//这个代码不要懵,换汤不换药,相当于分步执行以下4步。
// io.SetIMUProcessFunc();
// io.SetGNSSProcessFunc();
// io.SetOdomProcessFunc();
// io.Go();
// 为什么要这样写?你看不懂是我最大的愿望,哈哈,我的理解(有学过javaweb的同学就能很好的理解了,IDEA可以方便生成一个类的Get和Set,其实就是给客户开放了便捷的操作接口)
// 那高博连起来写是作什么?他其实就是想让你看不懂,没有什么其他,哈哈!原理同stdout的 << 符号。
io.SetIMUProcessFunc([&](const sad::IMU& imu) {
/// IMU 处理函数
if (!imu_init.InitSuccess()) {
imu_init.AddIMU(imu);
return;
}
/// 需要IMU初始化
if (!imu_inited) {
// 读取初始零偏,设置ESKF
sad::ESKFD::Options options;
// 噪声由初始化器估计
options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());
imu_inited = true;
return;
}
if (!gnss_inited) {
/// 等待有效的RTK数据
return;
}
/// GNSS 也接收到之后,再开始进行预测
eskf.Predict(imu);
/// predict就会更新ESKF,所以此时就可以发送数据
auto state = eskf.GetNominalState();
if(ui){
ui->UpdateNavState(state);
}
/// 记录数据以供绘图
save_result(fout, state);
usleep(1e3);
})
.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
/// GNSS 处理函数
if (!imu_inited) {
return;
}
sad::GNSS gnss_convert = gnss;
if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
return;
}
/// 去掉原点
if (!first_gnss_set) {
origin = gnss_convert.utm_pose_.translation();
first_gnss_set = true;
}
gnss_convert.utm_pose_.translation() -= origin;
// 要求RTK heading有效,才能合入ESKF
eskf.ObserveGps(gnss_convert);
auto state = eskf.GetNominalState();
if (ui) {
ui->UpdateNavState(state);
}
save_result(fout, state);
gnss_inited = true;
})
.SetOdomProcessFunc([&](const sad::Odom& odom) {
/// Odom 处理函数,本章Odom只给初始化使用
imu_init.AddOdom(odom);
if (FLAGS_with_odom && imu_inited && gnss_inited) {
eskf.ObserveWheelSpeed(odom);
}
})
.Go();
while (ui && !ui->ShouldQuit()) {
usleep(1e5);
}
if (ui) {
ui->Quit();
}
return 0;
}
关于ESKF部分,后面再详细分解。
https://zhuanlan.zhihu.com/p/441182819