目标跟踪：无迹卡尔曼滤波
在上一篇文章中，我们讨论分析了拓展卡尔曼滤波(EKF)，拓展卡尔曼滤波由于其在非线性话的过程中省略掉了高阶项，人为的引入了线性化误差。同时，面对状态向量维数较大，非线性环节复杂的过程，其雅克比矩阵比较难求，导致拓展卡尔曼滤波计算量增大，实时性降低。
因此我们在这一篇内容介绍另外一种非线性环节的处理方法，即无迹变换以及相应的无迹卡尔曼滤波。
写在最前面
我个人认为这一篇文章对于刚开始学的同学难度较大，因为无迹卡尔曼滤波采用了线性卡尔曼滤波的架构，所以在开始学这一部分的内容，需要对之前讲到的线性卡尔曼滤波熟知。(可以查看本专栏前面的文章)
无迹变换
提示：这一部分非常重要，是核心部分，所以一定要耐心看完。
小编总结：
无迹变换是对非线性环节的处理。假如有这样一个非线性环节y=f(x)，我们在x附近取了很多Sigma点[x1,x2,...,xn],所以就有[y1,y2,...,yn]=[f(x1),f(x2),...,f(xn)]，然后y=w1*y1+w2*y2+...+wn*yn。
希望上面这点内容可以使你更容易理解无迹变换
无迹卡尔曼滤波
我们考虑如下系统：
对应的无迹卡尔曼滤波算法如下：
Matalb仿真分析
为了和之前讲到的拓展卡尔曼滤波进行分析对，我们仿真参数和模型与拓展卡尔曼滤波仿真参数一致，需要的小伙伴可以查看本专栏有关拓展卡尔曼滤波的文章。
在这里肯定会有很多小伙伴充满了疑问？
从仿真结果来看，拓展卡尔曼滤波和无迹卡尔曼滤波一样呀，无迹卡尔曼滤波也没有减少拓展卡尔曼滤波的误差呀！
小编认为：我们的非线性环节较为简单，拓展卡尔曼滤波非线性化的时候引入的误差比较小。
Matalb仿真代码
clear;close all;
T = 1;
N = 60;
F = [1,T,0,0;
0,1,0,0;
0,0,1,T;
0,0,0,1]; %二维CV模型
X0 = [100 2 200 20]';Qk = diag([0.1 0.01 0.1 0.01]);Rk = 0.1;
P0 = diag([0.1 0.01 0.1 0.01]);
Xstation = [0;0];
Xk = Track(X0,Qk,F,N/T);Zk = Measurements(Xk,Rk,Xstation); % 观测方程为非线性表达，所以直接在函数里面进行计算
[Xekf] = EKF(X0,F,Zk,Qk,Rk,P0,Xstation);
lamda = 1;alpha = 0.01;belta = 2;
[Xukf] = UKF(X0,F,Zk,Qk,Rk,P0,Xstation,lamda,alpha,belta);
figure()
plot(Xk(1,:),Xk(3,:),'LineWidth',1);hold on;
plot(Xekf(1,:),Xekf(3,:),'LineWidth',1);hold on;
plot(Xukf(1,:),Xukf(3,:),'LineWidth',1);grid on;
legend('真实轨迹','EKF估计航迹','UKF估计航迹')
delta1 = sqrt((Xk(1,:)-Xekf(1,:)).^2+(Xk(3,:)-Xekf(3,:)).^2);
delta2 = sqrt((Xk(1,:)-Xukf(1,:)).^2+(Xk(3,:)-Xukf(3,:)).^2);
sumdelta1 = sum(delta1)/N;
sumdelta2 = sum(delta2)/N;
disp(['EKF平均误差：',num2str(sumdelta1)])
disp(['UKF平均误差：',num2str(sumdelta2)])
figure()
plot(delta1,'*');hold on
plot(delta2,'O');grid on
xlabel('时间/s');ylabel('距离均方误差');
legend('拓展Kalman滤波均方误差','UKF滤波均方误差')
%% 子函数
function Xk = Track(X0,Qk,F,N)
% 目标的真实航迹
%%输入参数
% X0 - 状态初值
% Qk - 过程噪声协方差
% Fx - 转移矩阵
% N - 仿真总结拍
%%输出参数
% Xk - 真实航迹
n_x = length(X0); % 状态向量维度
Xk = zeros(n_x,N);Xk(:,1) = X0;
for ii = 2:N
Xk(:,ii) = F*Xk(:,ii-1) + Qk*randn(n_x,1);
end
end
%--------------------------------------------------------------------------
function Zk = Measurements(Xk,Rk,Xstation)
% 传感器量测
%输入参数
% Xk - 真实航迹
% Rk - 量测噪声协方差
% Xstation - 观测站的坐标
%输出参数
% Zk - 传感器量测值
N = length(Xk(1,:)); % 仿真总节拍
Zk = zeros(1,N);
for ii = 1:N
Zk(ii) = atan((Xk(3,ii)-Xstation(2))/(Xk(1,ii)-Xstation(1)))+Rk*randn(1);
end
end
%--------------------------------------------------------------------------
function [Xekf] = EKF(X0,F,Zk,Qk,Rk,P0,Xstation)
% 拓展卡尔曼滤波
%输入参数
% X0 - 滤波初值
% F - 转移矩阵
% Zk - 传感器量测
% Qk - 过程噪声协方差
% Rk - 量测噪声协方差
% Xstation - 观测站坐标
%输出参数
% Xekf - 状态估计
n_x = length(X0); % 状态向量维数
N = length(Zk); % 仿真总节拍
Xekf = zeros(n_x,N);Xekf(:,1) = X0;
Pk_Last = P0; % 协方差初始化
for ii = 2:N
Xekf_hat = F*Xekf(:,ii-1); % 先验估计
Pk = F*Pk_Last*F' + Qk; % 先验协方差矩阵
H = [-(Xekf_hat(3)-Xstation(2))/((Xekf_hat(1)-Xstation(1))^2+(Xekf_hat(3)-Xstation(2))^2),0,...
(Xekf_hat(1)-Xstation(1))/((Xekf_hat(1)-Xstation(1))^2+(Xekf_hat(3)-Xstation(2))^2),0];%即为所求一阶近似
Kk = Pk*H'/(H*Pk*H'+Rk); % 卡尔曼增益
Zzk = atan((Xekf_hat(3)-Xstation(2))/(Xekf_hat(1)-Xstation(1)));
Xekf(:,ii) = Xekf_hat + Kk*(Zk(ii)-Zzk); % 后验估计
Pk_Last = (eye(n_x)-Kk*H)*Pk; % 更新误差协方差
end
end
%--------------------------------------------------------------------------
function [Xukf] = UKF(X0,F,Zk,Qk,Rk,P0,Xstation,lamda,alpha,belta)
% 无迹卡尔曼滤波
%输入参数
% X0 - 滤波初值
% F - 转移矩阵
% Zk - 传感器量测
% Qk - 过程噪声协方差
% Rk - 量测噪声协方差
% Xstation - 观测站坐标
% lamda - UT变化中的缩放比例参数(用来降低总的预测误差)
% alpha - UT变换中的参数(控制了采样点的分布状态)
% belta - UT变换中的参数(非负权系数，合并非线性环节中的高阶项的影响)
%输出参数
% Xukf - 状态估计
x_n = length(X0); % 状态向量维数
N = length(Zk); % 仿真总节拍
Xukf = zeros(x_n,N);Xukf(:,1) = X0;
Pk_Last = P0; % 协方差初始化
for ii = 2:N
Xukf_hat = F*Xukf(:,ii-1); % 先验估计
Pk = F*Pk_Last*F' + Qk; % 先验协方差矩阵
% 下面对观测环节进行线性化
%根据先验估计，使用无迹变换，产生Sigma点集以及对应的权值
XgamaP1 = zeros(x_n,x_n);
XgamaP2 = zeros(x_n,x_n);
cho = (chol(Pk*(x_n+lamda)))'; % R = chol(A),将A分解成满足A = A'*A
for jj = 1:x_n
XgamaP1(:,jj) = Xukf_hat + cho(:,jj);
XgamaP2(:,jj) = Xukf_hat - cho(:,jj);
end
Xsigma = [Xukf_hat,XgamaP1,XgamaP2]; % sigma点集
Wm = zeros(1,2*x_n+1);
Wc = zeros(1,2*x_n+1);
for jj = 1:2*x_n+1
Wm(jj) = lamda/(2*(x_n+lamda));
Wc(jj) = lamda/(2*(x_n+lamda));
end
Wm(1) = lamda/(x_n+lamda);
Wc(1) = Wm(1)+(1-alpha^2+belta);
% 将Sigma点集代入观测方程，得到预测的观测量
Zpredict = zeros(1,2*x_n+1);
for jj = 1:2*x_n+1
Zpredict(jj) = atan((Xsigma(3,jj)-Xstation(2))/(Xsigma(1,jj)-Xstation(1)));
end
% 根据预测的观测量，通过加权求和得到系统预测的均值以及协方差
Zpred = 0;
for jj = 1:2*x_n+1
Zpred = Zpred + Wm(jj)*Zpredict(jj);
end
Pzz = 0;
for jj = 1:2*x_n+1
Pzz = Pzz + Wc(jj)*(Zpredict(jj)-Zpred)*(Zpredict(jj)-Zpred)';
end
Pzz = Pzz + Rk;
Pxz = zeros(x_n,1);
for jj = 1:2*x_n+1
Pxz = Pxz + Wc(jj)*(Xsigma(:,jj)-Zpred)*(Zpredict(jj)-Zpred)';
end
% 计算Kalman增益矩阵
Kk = Pxz/Pzz;
% 系统状态更新和协方差更新
Xukf(:,ii) = Xukf_hat + Kk*(Zk(ii)-Zpred); % 状态更新
Pk_Last = Pk-Kk*Pzz*Kk';%方差更新
end
end