容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—I)
容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分)
原创不易，路过的各位大佬请点个赞
作者:[email protected]
备注：
容积卡尔曼滤波算法；CKF；Cubature Kalman Filter
两种CKF算法：加性噪声CKF和非加性噪声CKF
本博客主要讲解“加性噪声条件下的容积卡尔曼滤波算法”推导结果
matlab实现;
目标跟踪仿真
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）
无迹卡尔曼滤波UKF—目标跟踪中的应用
- 容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分)
-
- 1、带加性噪声的容积卡尔曼滤波算法CKF
-
- 1.1 问题描述（离散时间非线性系统描述）
- 1.2 容积点集
- 1.3 加性噪声容积卡尔曼滤波CKF算法
- 2. 仿真实验
-
- 4.1 主函数
- 4.2 UKF函数
- 4.3 量测函数
容积卡尔曼滤波思考：
为了克服无迹卡尔曼滤波在高维情况下出现滤波精度低的问题，Arasaratnam 和Haykin基于Caubature求积分变换，提出了容积卡尔曼滤波CKF方法。后来众多学者又基于CKF，提出了很多改进版本，如平方根CKF。
对于高斯分布下的非线性滤波问题，实际上就求后验期望的积分。由于被积分函数表现为非线性后验分布与高斯概率密度的乘积，因此一般很难得到解析解。这也是线性系统下该积分可以得到解析解，即著名的卡尔曼滤波算法。
E [ x ∣ z ] = ∫ R f ( x ) exp ( − x T x ) d x E[x|z]=\int_Rf(x)\exp(-x^Tx)dx E[x∣z]=∫Rf(x)exp(−xTx)dx
因此针对该非线性函数的积分问题，营运产生了众多基于数值积分的滤波算法。如UKF通过确定性采样来传播分布的一二阶矩（均值和方差）。而CKF作为看另一种求积分近似方法，利用球面径向规则。
CKF和UKF 总结：
当取 κ = 0 \kappa=0 κ=0时， CKF 和 UKF 的估计精度相同，但鉴于 CKF 采样点少，实时性
比 UKF 好，故应选用 CKF 滤波算法；
当 n ≤ 2 n\leq2 n≤2时即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波
算法；
当 n = 2 n=2 n=2时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更
好，应选用 CKF 滤波算法；
当 n ≥ 3 n\geq3 n≥3时即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。
下面介绍加性噪声下的容积卡尔曼滤波算法
1、带加性噪声的容积卡尔曼滤波算法CKF
1.1 问题描述（离散时间非线性系统描述）
考虑带加性噪声的一般非线性系统模型，
x k = f ( x k − 1 ) + w k − 1 z k = h ( x k ) + v k (1) x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1} xk=f(xk−1)+wk−1zk=h(xk)+vk(1)
其中 x k x_k xk为 k k k时刻的目标状态向量。 z k z_k zk为 k k k时刻量测向量（传感器数据）。这里不考虑控制器 u k u_k uk。 w k {w_k} wk和 v k {v_k} vk分别是过程噪声序列和量测噪声序列，并假设 w k w_k wk和 v k v_k vk为零均值高斯白噪声，其方差分别为 Q k Q_k Qk和 R k R_k Rk的高斯白噪声，即 w k ∼ ( 0 , Q k ) w_k\sim(0,Q_k) wk∼(0,Qk), v k ∼ ( 0 , R k ) v_k\sim(0,R_k) vk∼(0,Rk)，且满足如下关系(线性高斯假设)为：
E [ w i v j ′ ] = 0 E [ w i w j ′ ] = 0 i ≠ j E [ v i v j ′ ] = 0 i ≠ j \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned} E[wivj′]E[wiwj′]E[vivj′]=0=0i=j=0i=j
1.2 容积点集
{ ξ i , w i } \{\xi_i, w_i\} { ξi,wi}
利用三阶球面径向规则计算标准高斯加权积分，即
E [ x ∣ z ] = ∫ R f ( x ) N ( x ; 0 , I ) d x ≈ ∑ i = 1 m w i f ( ξ i ) E[x|z]=\int_Rf(x)\mathcal{N}(x; 0,I)dx\approx\sum_{i=1}^{m}w_if(\xi_i) E[x∣z]=∫Rf(x)N(x;0,I)dx≈i=1∑mwif(ξi)
式中
ξ i = m 2 [ 1 ] i , i = 1 , 2 , ⋯ , m = 2 n w i = 1 m , i = 1 , 2 , ⋯ , m = 2 n (2) \begin{aligned} \xi_i&=\sqrt{\frac{m}{2}}[\mathbf{1}]_i, i=1,2,\cdots,m=2n\\ w_i&=\frac{1}{m}, i=1,2,\cdots,m=2n \end{aligned} \tag{2} ξiwi=2m[1]i,i=1,2,⋯,m=2n=m1,i=1,2,⋯,m=2n(2)
[ 1 ] [\mathbf{1}] [1] 表示 n n n（ n n n维状态维数） 维空间的点集，即
1.3 加性噪声容积卡尔曼滤波CKF算法
1.） 初始化
步骤一：
给定 k − 1 k-1 k−1时刻的状态估计和协方差矩阵
x ^ k − 1 ∣ k − 1 , P k − 1 ∣ k − 1 , Q k − 1 , R k − 1 \hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1} x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1
当为 k = 0 k=0 k=0时刻时，假设 x 0 ∼ ( x ˉ 0 , P 0 ) , Q 0 , R 0 x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0} x0∼(xˉ0,P0),Q0,R0，则滤波器最优初始化为
x ^ 0 ∣ 0 = x ˉ 0 P 0 ∣ 0 = P 0 \hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0 x^0∣0=xˉ0P0∣0=P0
2. ) 状态预测
步骤二： 根据 k − 1 k-1 k−1时刻的估计 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k−1∣k−1和方差 P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1,利用（2）式产生容积点
S k − 1 ∣ k − 1 = P k − 1 ∣ k − 1 X k − 1 ∣ k − 1 i = x ^ k − 1 ∣ k − 1 + S k − 1 ∣ k − 1 ξ i \begin{aligned} &S_{k-1|k-1}=\sqrt{P_{k-1|k-1}}\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}_{k-1|k-1}+S_{k-1|k-1}\xi_i \end{aligned} Sk−1∣k−1=Pk−1∣k−1Xk−1∣k−1i=x^k−1∣k−1+Sk−1∣k−1ξi
步骤三： 根据非线性模型进行容积点的非线性传播
X k ∣ k − 1 i = f ( X k − 1 ∣ k − 1 i ) ， i = 1 , 2 , ⋯ , m \mathcal{X}^i_{k|k-1}=f(\mathcal{X}^i_{k-1|k-1})，i=1,2,\cdots,m Xk∣k−1i=f(Xk−1∣k−1i)，i=1,2,⋯,m
步骤四： 状态一步预测和预测方差
x ^ k ∣ k − 1 = 1 m ∑ i = 1 m X k ∣ k − 1 i P k ∣ k − 1 = 1 m ∑ i = 0 m ( X k ∣ k − 1 i − x ^ k ∣ k − 1 ) ( X k ∣ k − 1 i − x ^ k ∣ k − 1 ) ′ + Q k \begin{aligned} &\hat{x}_{k|k-1}=\frac{1}{m}\sum_{i=1}^{m}\mathcal{X}^i_{k|k-1}\\ &P_{k|k-1}=\frac{1}{m}\sum_{i=0}^{m}(\mathcal{X}^i_{k|k-1}-\hat{x}_{k|k-1})(\mathcal{X}^i_{k|k-1}-\hat{x}_{k|k-1})'+Q_k \end{aligned} x^k∣k−1=m1i=1∑mXk∣k−1iPk∣k−1=m1i=0∑m(Xk∣k−1i−x^k∣k−1)(Xk∣k−1i−x^k∣k−1)′+Qk
3. ) 状态更新
步骤五： 根据 k k k时刻的估计 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^k∣k−1和方差 P k ∣ k − 1 P_{k|k-1} Pk∣k−1，计算容积点
S k ∣ k − 1 = P k ∣ k − 1 X ^ k ∣ k − 1 i = x ^ k ∣ k − 1 + S k ∣ k − 1 ξ i \begin{aligned} &S_{k|k-1}=\sqrt{P_{k|k-1}}\\ &\hat{\mathcal{X}}^i_{k|k-1}=\hat{x}_{k|k-1}+S_{k|k-1}\xi_i \end{aligned} Sk∣k−1=Pk∣k−1X^k∣k−1i=x^k∣k−1+Sk∣k−1ξi
步骤六： 将 X k ∣ k − 1 i \mathcal{X}^i_{k|k-1} Xk∣k−1i点通过量测方程传播，
Z ^ k ∣ k − 1 i = h ( X ^ k ∣ k − 1 i ) ， i = 1 , 2 , ⋯ , m \hat{\mathcal{Z}}^i_{k|k-1}=h(\hat{\mathcal{X}}^i_{k|k-1})，i=1,2,\cdots,m Z^k∣k−1i=h(X^k∣k−1i)，i=1,2,⋯,m
步骤七： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
z k ∣ k − 1 = 1 m ∑ i = 1 m Z ^ k ∣ k − 1 i S k = 1 m ∑ i = 1 m ( Z ^ k ∣ k − 1 i − z k ∣ k − 1 ) ( Z ^ k ∣ k − 1 i − z k ∣ k − 1 ) ′ + R k C k = 1 m ∑ i = 1 m ( X k ∣ k − 1 i − x k ∣ k − 1 ) ( Z ^ k ∣ k − 1 i − z k ∣ k − 1 ) ′ K k = C k S k − 1 \begin{aligned} &z_{k|k-1}=\frac{1}{m}\sum_{i=1}^{m}\hat{\mathcal{Z}}^i_{k|k-1}\\ &S_{k}=\frac{1}{m}\sum_{i=1}^{m}(\hat{\mathcal{Z}}^i_{k|k-1}-z_{k|k-1})(\hat{\mathcal{Z}}^i_{k|k-1}-z_{k|k-1})'+R_k\\ &C_{k}=\frac{1}{m}\sum_{i=1}^{m}(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\hat{\mathcal{Z}}^i_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned} zk∣k−1=m1i=1∑mZ^k∣k−1iSk=m1i=1∑m(Z^k∣k−1i−zk∣k−1)(Z^k∣k−1i−zk∣k−1)′+RkCk=m1i=1∑m(Xk∣k−1i−xk∣k−1)(Z^k∣k−1i−zk∣k−1)′Kk=CkSk−1
步骤八：
x ^ k ∣ k = E [ x k ∣ Z k ] = x ^ k ∣ k − 1 + K z ( z k − z ^ k ∣ k − 1 ) P k ∣ k = cov ( x ~ k ∣ k ) = P k ∣ k − 1 − K k S k K k ′ (5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5} x^k∣kPk∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)=cov(x k∣k)=Pk∣k−1−KkSkKk′(5)
其中估计误差为
x ~ k ∣ k = x k − x ^ k ∣ k \widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k} x k∣k=xk−x^k∣k
以上公式为带加性噪声非线性系统的容积卡尔曼滤波算法。其中注意： 在求交互协方差 C k C_{k} Ck时，使用的容积点为 X k ∣ k − 1 i \mathcal{X}^i_{k|k-1} Xk∣k−1i， 而非 X ^ k ∣ k − 1 i \hat{\mathcal{X}}^i_{k|k-1} X^k∣k−1i。
2. 仿真实验
以二维雷达目标跟踪实列做为仿真案列。
说明:
1.二维仿真代码也可以在上面的连接中直接下载，
2.将UKF函数保存，文件名“fun_2CKF.m”
3.将量测函数保存，文件名“measurements.m”
4. 运行下面的主函数
5. 注意将这三个文件保存在一个文件夹下
4.1 主函数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by: D. Zhang
% date: 2020/4
% 容积卡尔曼滤波，目标跟踪
% 二维目标跟踪问题
% 线性CV目标模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% initial parameter
n=4; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=200; %运行总时刻
MC=100; %蒙特卡洛次数
chan=1; %滤波器通道，这里只有一个滤波器
w_mu=[0,0]'; % mean of process noise
v_mu=[0,0]'; % mean of measurement noise
%% target model
%covariance of process noise
q_x=0.01; %m/s^2
q_y=q_x;
Qk=diag([q_x^2,q_y^2]);
% state matrix
Fk= [1 T 0 0
0 T 0 0
0 0 1 T
0 0 0 T]; %
Gk= [ T^2/2 0
T 0
0 T^2/2
0 T ]; %
%量测模型
sigma_r(1)=80; sigma_b(1)=60e-3; % covariance of measurement noise (radar)
Rk=diag([sigma_r(1)^2, sigma_b(1)^2]);
xp=[0,0,0,0];%雷达位置
%% 定义存储空间
sV=zeros(n,N,MC); % 状态
eV=zeros(n,N,MC,chan); %估计
PV=zeros(n,n,N,MC,chan);%协方差
rV=zeros(2,N,MC,M); % %量测
for i=1:MC
sprintf('rate of process:%3.1f%%',(2*i)/(4*MC)*100)
% 初始状态的均值和方差
x=[100,15,100,15]';
P_0=diag([1e5,10^1,1e5,10^1]);
xk_ckf=x; Pk_ckf=P_0; % P0|0 x0|0
x0=mvnrnd(x,P_0); % 初始状态
%x0=(x+normrnd(0,0.001)')';
x=x0';
for k=1:N
%% %%%%%%% 目标模型和雷达量测模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 目标模型CV
w=mvnrnd(w_mu',Qk)';
x=Fk*x+Gk*w;
sV(:,k,i)=x;
% 雷达量测模型，M=1，表示一个雷达
for m=1:M
v=normrnd(v_mu,[sigma_r(m); sigma_b(m)]);
[r,b] = measurements(x,xp);%r=距离，b=角度
rm=r+v(1);
bm=b+v(2);
rV(:,k,i,m)=[rm,bm]';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% %%%%%%%%% CKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xk_ckf,Pk_ckf] = fun_2CKF(xk_ckf,Pk_ckf,Fk,Gk,rV(:,k,i,1),Qk,Rk, xp);
PV(:,:,k,i,1)=Pk_ckf;
eV(:,k,i,1)=xk_ckf;
%%%%%%%%% end centralized fusion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
figure
plot(sV(1,:,1),sV(3,:,1),'--k',eV(1,:,1,1),eV(3,:,1,1),'-r')
xlabel('m');ylabel('m');
legend('State','CKF')
title('跟踪轨迹')
figure;
ii=1:N;
plot(ii,sV(1,:,1),'--k',ii,eV(1,:,1,1),'r');
xlabel('t/s');ylabel('m');%X轴的跟踪轨迹
legend('X-轴','CKF')
title('X轴的跟踪轨迹')
%P_real=cell(MC,chan);
P_r=0;
for i=1:MC
sprintf('rate of process:%3.1f%%',(3*MC+i)/(4*MC)*100)
for k=1:N
for c=1:chan
error(:,c)=sV(:,k,i,1)-eV(:,k,i,c);
% RMSE
error2(:,c)=error(:,c).^2;
error2_dis(c)=error2(1,c)+error2(3,c);
error2_vel(c)=error2(2,c)+error2(4,c);
position(k,i,c)=error2_dis(c);
velocity(k,i,c)=error2_vel(c);
end
end
end
%% RMSE
for c=1:chan
rms_position(:,c)=sqrt(sum(position(:,:,c),2)./MC);
rms_velocity(:,c)=sqrt(sum(velocity(:,:,c),2)./MC);
end
figure;%position
plot(ii,rms_position(ii,1),'.-r','LineWidth',0.7);
legend('CKF')
xlabel('t/s');ylabel('RMSE');
title('position-RMS analyze')
figure;%position
plot(ii,rms_velocity(ii,1),'.-r','LineWidth',0.7);
legend('CKF')
xlabel('t/s');ylabel('RMSE');
title('velocity-RMS analyze')
4.2 UKF函数
function [xk,Pk] = fun_2CKF(xk,Pk,Fk,Gk,Zm,Qk,Rk, xp);
%%
%CKF filter
%%
zk=Zm(:,:); %stacked measurement:rm bm em
%目标模型
% xkk=Fk*xk;
% Pkk=Fk*Pk*Fk'+Gk*Qk*Gk';
nx=4;%维数为4
mx=2*nx;
%%%%%%%%%%%%%%%%%%%%%%%
Ix=[eye(nx), -eye(nx)];
xi=zeros(nx, mx);
for i=1:mx
xi(:,i)=sqrt(mx/2)*Ix(:,i);
end
SPk=chol(Pk)';
%。产生容积点，通过目标函数传播容积点
for i=1:mx
Xsigma(:,i)=xk+SPk*xi(:,i);
Xgama(:,i)=Fk*Xsigma(:,i);
end
xkk=sum(Xgama,2)/mx;
Pkk_i=zeros(nx,nx,mx);
for i=1:mx
Pkk_i(:,:,i)=(Xgama(:,i)-xkk)*(Xgama(:,i)-xkk)';
end
Pkk=sum(Pkk_i, 3)/mx + Gk*Qk*Gk';
%产生xkk的容积点
SPkk=chol(Pkk)';
for i=1:mx
Zsigma(:,i)=xkk+SPkk*xi(:,i);
end
%通过量测函数传播容积点
for i=1:mx
[z1,z2] = measurements(Zsigma(:,i), xp); Zgama(:,i)=[z1,z2]';
end
zkk=sum(Zgama,2)/mx;
Sk_i=zeros(2,2,mx);
for i=1:mx
Sk_i(:,:,i)=(Zgama(:,i)-zkk)*(Zgama(:,i)-zkk)';
end
Sk=sum(Sk_i, 3)/mx + Rk;
Ck_i=zeros(4,2,mx);for i=1:mx; Ck_i(:,:,i)=(Xgama(:,i)-xkk)*(Zgama(:,i)-zkk)';end;
Ck=sum(Ck_i, 3)/mx;
%更新
xk=xkk+Ck*inv(Sk)*(zk-zkk);
Pk=Pkk-Ck*inv(Sk)*Ck';
end
4.3 量测函数
function [fz1,fz2] = measurements(fx,fxp)
%极坐标量测，二维雷达量测
%距离量测
fz1=sqrt((fx(1)-fxp(1))^2+((fx(3)-fxp(3))^2));
%方位角
fz2=atan2((fx(3)-fxp(3)),(fx(1)-fxp(1)));
end