文章目录阶段二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
文章目录阶段二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
