【4】lightning_lm项目-LIO 前端 - 深入学习 ESKF 滤波器-北京尧图网络

文章目录阶段二LIO 前端 - 深入学习 ESKF 滤波器一、ESKF 整体结构二、核心函数 1Predict() - IMU 预测三、核心函数 2Update() - 激光观测更新四、ESKF 的几个关键技巧总结五、ESKF 与普通卡尔曼滤波的区别重点总结下一步阶段二LIO 前端 - 深入学习 ESKF 滤波器深入学习 ESKF误差状态卡尔曼滤波器的具体实现。这是整个 LIO 系统的核心算法一、ESKF 整体结构看 eskf.hpp先看类的成员变量NavState x_;// 状态量名义状态CovType P_CovType::Identity();// 协方差矩阵CustomObsFunction lidar_obs_func_;// 激光观测函数booluse_aa_false;// 是否用 Anderson 加速关键点 x_ 是名义状态nominal state估计的是「误差状态」然后加到名义状态上这种方法叫做「误差状态卡尔曼滤波」Error-State Kalman Filter二、核心函数 1Predict() - IMU 预测看 eskf.ccvoidESKF::Predict(constdoubledt,constProcessNoiseTypeQ,constVec3dgyro,constVec3dacce);这一步做什么用 IMU 数据预测下一时刻的状态和协方差。步骤详解计算运动方程Eigen::Matrixdouble,NavState::full_dim,1f_x_.get_f(gyro,acce);Eigen::Matrixdouble,NavState::full_dim,state_dim_f_x_x_.df_dx(acce);Eigen::Matrixdouble,NavState::full_dim,process_noise_dim_f_w_x_.df_dw();get_f() 运动方程的导数df_dx() 运动方程对状态的雅可比df_dw() 运动方程对噪声的雅可比名义状态更新NavState x_beforex_;x_.oplus(f_,dt);// 用中值积分更新名义状态计算状态转移矩阵这里代码有点复杂因为要处理 SO(3) 旋转的特殊情况协方差预测P_(F_x1_)*P_*(F_x1_).transpose()(dt*f_w_final)*Q*(dt*f_w_final).transpose();P_*options_.predict_cov_inflation_;// 稍微膨胀一下协方差更鲁棒SymmetrizeAndFloorCovariance(P_,options_.min_cov_diag_);// 保持对称并防止协方差太小三、核心函数 2Update() - 激光观测更新看 eskf.cc这是最复杂、最关键的函数这是一个迭代优化的过程注意循环从 i -1 开始为什么第 -1 次迭代计算初始残差不更新状态后面迭代真正更新状态迭代优化步骤计算观测模型if(obsObsType::LIDAR){lidar_obs_func_(x_,custom_obs_model_);// 调用 ObsModel()}这里会调用之前看到的 LaserMapping::ObsModel() 函数检查收敛if(use_aa_i-1...residual变大了){x_last_x;// 如果残差变大回退到上一步break;}计算误差状态StateVecType dxx_.boxminus(start_x);// 当前状态与起始状态的差处理 SO(3) 的误差因为旋转是在流形上需要特殊处理for(autoit:x_.SO3_states_){intidxit.idx_;Vec3d seg_SO3dx.block3,1(idx,0);Mat3d res_temp_SO3math::A_matrix(seg_SO3).transpose();dx_current.block3,1(idx,0)res_temp_SO3*dx.block3,1(idx,0);// 同样更新 P 矩阵...}处理退化情况这是一个非常高级的技巧Eigen::SelfAdjointEigenSolverMat6deigen_solver(HTH_sym);constVec6d eigen_valueseigen_solver.eigenvalues();constdoublemax_eigen_valuestd::max(1e-12,eigen_values.maxCoeff());constdoubledegeneracy_thresholdmax_eigen_value*options_.degeneracy_threshold_ratio_;// 找出可观测的方向Vec6d observable_maskVec6d::Zero();for(intk0;kobservable_mask.size();k){if(eigen_values(k)degeneracy_threshold){observable_mask(k)1.0;}}// 只在可观测的方向上更新constMat6d observable_projectoreigen_vectors*observable_mask.asDiagonal()*eigen_vectors.transpose();constMat6d HTH_effobservable_projector*HTH_sym*observable_projector;constVec6d HTr_effobservable_projector*HTr;为什么要这样有时候某些方向上观测很差退化如果强行更新会把状态改坏所以只在观测好的方向上更新计算卡尔曼增益并更新CovType P_temp(P_/R).inverse();P_temp.blockpose_obs_dim_,pose_obs_dim_(0,0)HTH_eff;CovType Q_invP_temp.inverse();// 计算增益K_rQ_inv.templateblockstate_dim_,pose_obs_dim_(0,0)*HTr_eff;K_H.setZero();K_H.templateblockstate_dim_,pose_obs_dim_(0,0)Q_inv.templateblockstate_dim_,pose_obs_dim_(0,0)*HTH_eff;// 更新状态dx_currentK_r(K_H-Eigen::Matrixdouble,state_dim_,state_dim_::Identity())*dx_current;限制更新步长constdoubledx_translationdx_current.head3().norm();constdoubledx_rotation_degdx_current.segment3(3).norm()*180.0/M_PI;if(dx_translationoptions_.max_update_translation_step_||dx_rotation_degoptions_.max_update_rotation_step_deg_){x_start_x;// 如果步长太大拒绝更新return;}这很重要防止一次更新把状态改飞了Anderson 加速if(!use_aa_){x_x_.boxplus(dx_current);// 普通更新}else{x_x_.boxplus(dx_current);if(i-1){aa_.init(dx_current);}else{autodx_allx_.boxminus(start_x);autonew_dx_allaa_.compute(dx_all);// Anderson 加速x_start_x.boxplus(new_dx_all);}}Anderson Acceleration 是什么一种数值加速方法让迭代更快收敛利用过去几次的迭代结果预测一个更好的更新方向通常能让迭代次数减少一半检查收敛custom_obs_model_.converge_true;for(intj0;jstate_dim_;j){if(std::fabs(dx_current[j])limit_[j]){custom_obs_model_.converge_false;break;}}更新协方差L_P_;// ... 处理 SO(3) ...P_L_-K_H.blockstate_dim_,pose_obs_dim_(0,0)*P_.templateblockpose_obs_dim_,state_dim_(0,0);四、ESKF 的几个关键技巧总结五、ESKF 与普通卡尔曼滤波的区别重点总结Predict 过程 IMU 积分预测状态和协方差Update 过程迭代优化用激光观测修正状态关键技巧误差状态表示退化处理更新步长限制Anderson 加速协方差处理对称化、限幅、退化时膨胀下一步下次可以学习IMU 处理模块 imu_processing.hpp 点云预处理模块 pointcloud_preprocess.h/cc IVox 局部地图 ivox3d.h

【4】lightning_lm项目-LIO 前端 - 深入学习 ESKF 滤波器

相关推荐

Traxxas Rally+ROS实战：从遥控车到真实AGV的端到端开发指南

工业负载控制方案：TPD2015FN与STM32F415RG应用实践

Anaconda 与 Miniconda 2024.10 选型指南：从 3GB 到 500MB 的 4 种场景决策

更多资讯

1990-2025年各省统计年鉴面板数据无缺失填充版

Agent Zero 系统包管理：深入解析 system_packages.py 的 apt 锁重试机制

pykan 设备（Device）管理与 CUDA 加速实战：为 KAN 模型与数据集正确传递 device 参数

将 Agent Zero 插件发布到社区 Plugin Index：从仓库准备到 PR 合入的完整贡献指南

mimalloc 测试体系深入解析：从内部不变量检查到完整 API 面覆盖

Renovate postUpgradeTasks 怎么配置并在自托管环境允许执行

今日更新

51单片机开环控制磁阻传感器的硬件匹配与代码实现

AI Agent安全操作数据库：SQL注入防御与参数化查询实战

Android逆向实战：还原激励类App签到接口的签名算法

本周精选

AI驱动的自动化专利分析平台技术解析

LeetCode-Go 题解：1252. Cells with Odd Values in a Matrix（奇数值单元格计数）双解法剖析

在线答疑系统Java毕设实战：Spring Boot前后端分离与状态流转

本月热门

基于YOLO的焊缝缺陷检测：从数据标注到工业部署全流程实践

分布式服务的决策记录

创业产品的首版范围

读完文章，想聊聊您的网站？