%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 = []; % 结构体
%读取Radar_Lidar_Data2.csv文件保存到矩阵Data
Data = csvread('Radar_Lidar_Data1.csv',1,1);%起点为第2行第2列 612组数据
%将Data矩阵数据保存到data.csv文件中
csvwrite('Data.csv',Data);
%初始化状态，x是4维向量
if (Data(1,1) == 1)
x = [Data(1,2); Data(1,3); 0; 0];
else
x = [
Data(1,2)*cos(Data(1,3));
Data(1,2)*sin(Data(1,3));
Data(1,4)*cos(Data(1,3));
Data(1,4)*sin(Data(1,3))
];
end
%初始化状态协方差矩阵
P = diag([1,1,1000,1000]);
%状态转移矩阵，线性
F = [[1, 0, dt, 0];
[0, 1, 0, dt];
[0, 0, 1, 0];
[0, 0, 0, 1]];
%系统误差协方差矩阵
Q = [[(dt^2)/4,0,(dt^3)/2,0];
[0,(dt^2)/4,0,(dt^3)/2];
[(dt^3/2),0,(dt^2),0];
[0,(dt^3)/2,0,(dt^2)]];
%Lidar传感器观测矩阵，线性
H_L = [[1, 0, 0, 0];
[0, 1, 0, 0]];
%雷达传感器测量误差协方差矩阵
R_r = diag([0.09,0.005,0.09]);
%激光雷达传感器测量误差协方差矩阵
R_L = diag([0.0025, 0.0025]);
%4阶单位矩阵
I = eye(4);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% HCKF参数设置 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L = numel(x); %状态维数 4x1
W = [2/(2+L),1/(L+2)^2 * ones(1,2*L*(L-1)),(4-L)/(2*(L+2)^2) * ones(1,2*L)]; %容积点权重 1x33
lamda = sqrt(L + 2);
S1 = 1/sqrt(2) * [
[1,1,1,0,0,0];
[1,0,0,1,1,0];
[0,1,0,1,0,1];
[0,0,1,0,1,1];
];
S2 = 1/sqrt(2) * [
[1,1,1,0,0,0];
[-1,0,0,1,1,0];
[0,-1,0,-1,0,1];
[0,0,-1,0,-1,-1];
];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 开始迭代 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for n = 1:length(Data)
%Radar传感器
%非线性
if (Data(n,1) == 2)
%Radar测量数据
z = transpose(Data(n,2:4));
z_p = [ z(1) * cos(z(2));z(1) * sin(z(2));z(3) * cos(z(2));z(3) * sin(z(2))];
%HCKF 算法核心部分
%======================================================================%
%1.采样:由x0,P0产生容积点
gama1 = lamda * S1; % 4x6
gama2 = lamda * S2; % 4x6
A11 = chol(P) * gama1; % 4x6
A12 = chol(P) * gama2; % 4x6
Y1 = x(:,ones(1,L+2)); % 4x6
X11_cuba = [Y1 + A11,Y1 - A11]; % 4x12
X12_cuba = [Y1 + A12,Y1 - A12]; % 4x12
X13_cuba = [];
for i = 1:4
gama3 = lamda * I(:,i); % 4x1
A13 = chol(P) * gama3; % 4x1
X13_cuba = [X13_cuba,x + A13,x - A13]; % 4x8
end
X1_cuba = [x,X11_cuba,X12_cuba,X13_cuba]; % 4x33
%2.容积点经过线性映射
cols = size(X1_cuba,2); % 返回矩阵列数
x1_mean = zeros(L,1); % 加权均值:经过线性映射后均值
X1_ckf = zeros(L,cols); % 容积点经过线性映射
for k = 1 : cols
X1_ckf(:,k) = F * X1_cuba(:,k); % 容积点经过线性映射
x1_mean = x1_mean + W(k) * X1_ckf(:,k); % 加权均值:经过线性映射后均值
end
error1 = X1_ckf - x1_mean(:,ones(1,cols)); % 经过线性映射后偏差 4x33
P1_cov = error1 * diag(W) * transpose(error1) + Q; % 加权方差:经过线性映射后方差 4x4
%3.采样:由x1_mean,P1_cov产生容积点
gama1 = lamda * S1; % 4x6
gama2 = lamda * S2; % 4x6
A21 = chol(P1_cov) * gama1; % 4x6
A22 = chol(P1_cov) * gama2; % 4x6
Y2 = x1_mean(:,ones(1,L+2)); % 4x6
X21_cuba = [Y2 + A21,Y2 - A21]; % 4x12
X22_cuba = [Y2 + A22,Y2 - A22]; % 4x12
X23_cuba = [];
for i = 1:4
gama3 = lamda * I(:,i); % 4x1
A23 = chol(P1_cov) * gama3; % 4x1
X23_cuba = [X23_cuba,x1_mean + A23,x1_mean - A23]; % 4x8
end
X2_cuba = [x,X21_cuba,X22_cuba,X23_cuba]; % 4x33
%4.非线性测量方程
h = @(x)[sqrt(x(1)^2 + x(2)^2);
atan(x(2)/x(1));
(x(1)*x(3) + x(2)*x(4)) / sqrt(x(1)^2 + x(2)^2)];
%5.容积点经过非线性映射
x2_mean = zeros(L-1,1); % 加权均值:经过非线性映射后均值 3x1
X2_ckf = zeros(L-1,cols); % 容积点经过非线性映射 3x33
for k = 1 : cols
X2_ckf(:,k) = h(X2_cuba(:,k)); % 容积点经过非线性映射 3x33
x2_mean= x2_mean + W(k) * X2_ckf(:,k); % 加权均值:经过非线性映射后均值 3x1
end
error2 = X2_ckf - x2_mean(:,ones(1,cols)); % 经过非线性映射后偏差 3x33
P2_cov = error2 * diag(W) * transpose(error2) + R_r; % 加权方差:经过非线性映射后方差 3x3
%6.量测更新
P3_cov = error1 * diag(W) * transpose(error2); % 方差cross 4x3
K = P3_cov / P2_cov; % 卡尔曼增益 4x3
x = x1_mean + K * (z - x2_mean); % 状态均值更新
P = P1_cov - K * P2_cov * transpose(K); % 状态方差更新
%7.保存数据
HCKF_estimation = [HCKF_estimation;transpose(x)];
Radar_measurement = [Radar_measurement; transpose(z)];
Radar_measurement_p = [Radar_measurement_p;transpose(z_p)];
%======================================================================%
%Lidar传感器
%线性
else
%KF 算法核心部分
%======================================================================%
%1.预测更新状态、状态协方差阵
x = F * x;
P = F * P * transpose(F) + Q;
%2.Lidar测量数据
z = transpose(Data(n,2:3));
%3.测量更新
%更新卡尔曼增益矩阵
y = z - (H_L * x);
S = H_L * P * transpose(H_L) + R_L;
K = P * transpose(H_L) / S;
x = x + (K * y); % 更新状态
P = (I - (K * H_L)) * P; % 更新状态协方差阵
%4.保存状态值
HCKF_estimation = [HCKF_estimation;transpose(x)];
Lidar_measurement = [Lidar_measurement; transpose(z)];
%======================================================================%
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 数据可视化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1);
hold on;
plot(Data(:,5),Data(:,6),'k'); %真实状态
scatter(HCKF_estimation(:,1),HCKF_estimation(:,2),5,'filled','r'); %估计状态
scatter(Lidar_measurement(:,1),Lidar_measurement(:,2),5,'filled','blue'); %Lidar传感器测量
scatter(Radar_measurement_p(:,1),Radar_measurement_p(:,2),5,'filled','g'); %Radar传感器测量
legend('truth','HCKF Path result','Lidar Measurement','Radar Measurement'); %标注
title('位置估计');
axis square;
grid on;
hold off;
%位置x误差
figure(2);
hold on;
HCKF_x_error = Data(1:2:1223,5) - HCKF_estimation(1:2:1223,1); %位置x误差
plot(1:612,HCKF_x_error);
plot(1:612,Data(1:2:1223,5) - Radar_measurement_p(:,1));
legend('HCKFerror','Radarerror');
title('位置x误差');
grid on;
hold off;
%位置y误差
figure(3);
hold on;
HCKF_y_error = Data(1:2:1223,6) - HCKF_estimation(1:2:1223,2); %位置y误差
plot(1:612,HCKF_y_error);
plot(1:612,Data(1:2:1223,6) - Radar_measurement_p(:,2));
legend('HCKFerror','Radarerror');
title('位置y误差');
grid on
hold off;
%速度x误差
figure(4);
hold on;
HCKF_vx_error = Data(1:2:1223,7) - HCKF_estimation(1:2:1223,3); %速度x误差
plot(1:612,HCKF_vx_error);
plot(1:612,Data(1:2:1223,7) - Radar_measurement_p(:,3));
legend('HCKFerror','Radarerror');
title('速度x误差');
grid on
hold off;
%速度y误差
figure(5);
hold on;
HCKF_vy_error = Data(1:2:1223,8) - HCKF_estimation(1:2:1223,4); %速度y误差
plot(1:612,HCKF_vy_error);
plot(1:612,Data(1:2:1223,8) - Radar_measurement_p(:,4));
legend('HCKFerror','Radarerror');
title('速度y误差');
grid on
hold off;
%将数据保存到 HCKF_error.csv 文件中,用于软法比较
HCKF_error = [HCKF_x_error,HCKF_y_error,HCKF_vx_error,HCKF_vy_error];
csvwrite('HCKF_error.csv',HCKF_error);
%保存到结构体
hckf.estimation = HCKF_estimation;
hckf.error = HCKF_error;
hckf.measurement_R = Radar_measurement_p;
hckf.measurement_L = Lidar_measurement;
%结束计时
toc;
来源：CSDN
作者：Aircraft GNC
链接：https://blog.csdn.net/qq_43614222/article/details/104675771