【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 种场景决策

更多资讯

电工证需要考些什么才能快速拿证太忙怎么复习

石狮办理电工证在哪里怎么拿电子证书电工证太忙没时间复习别慌

黄冬升电工证怎么考？怕考不过白交培训费？电子证书流程全解析

遵义市电工证培训怎么操作？工地太忙根本没时间复习考试

电工证年审体检项目郑州报考避坑指南

低压电工证武清哪里考官方回应怕考不过白交培训费

今日更新

河南特种工八大员含金量高吗？别被中介坑了，这些关键点你要知道

电工证ps软件多少钱？工地太忙根本没时间复习考试怎么办

牡丹江报考高压电工证在哪考？新手避坑指南

本周精选

安监局电工证分数查询怎么查才能知道含金量高不高

新疆电工作业干活累吗良心建议助你避开考不过的坑

中牟县电工证怎么报名外省证能转吗

本月热门

面向复杂光照与角度的宠物面部识别鲁棒性增强策略

基于DRV8818与STM32的双极步进电机驱动方案实战解析

DRV8818PWPR+PIC18F45K80双极步进电机控制方案实战

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