高阶容积卡尔曼滤波——非线性HCKF-Matlab
% HCKF高阶容积卡尔曼滤波在自动驾驶车辆定位中的应用 % 参考文献 : % The High - Degree Cubature Kalman Filter ( Bin Jia , Ming Xin , and Yang Cheng ) % Unscented Kalman Filter Tutorial ( Gabriel A . Terejanu ) % 基于正交变换的五阶容积卡尔曼滤波导航算法 ( 何康辉，董朝阳 ) % ＥＫＦ、ＵＫＦ和ＣＫＦ的滤波性能对比研究 ( 常宇健，赵辰 ) % 组合导航原理与应用 ( 西北工业大学出版社 ) % 卡尔曼滤波与Matlab仿真 ( 北京航空航天大学出版社 ) % 本例子使用线性状态模型，因此加速度变成了干扰项 % Lidar传感器测量模型为线性模型，Radar传感器测模型为非线性模型 % 在Radar传感器中使用滤波算然为 HCKF , 在Lidar传感器中使用滤波算然为 KF clear ; clc ; tic ; % 计时 dt = 0.1 ; % 步长 % 分配空间 Radar_measurement = [ ] ; Radar_measurement_p = [ ] ; Lidar_measurement = [ ] ; HCKF_estimation = [ ] ; hckf = [ ] ; % 结构体 %