线性卡尔曼滤波算法及示例
鲁道夫·卡尔曼（Rudolf Emil Kalman），匈牙利裔美国数学家，1930年出生于匈牙利首都布达佩斯。1957年于哥伦比亚大学获得博士学位。1964年至1971年任职斯坦福大学。1971年至1992年任佛罗里达大学数学系统理论中心主任。1972起任瑞士苏黎世联邦理工学院数学系统理论中心主任。2009年获美国国家科学奖章。
一、给出离散时间线性动态系统和三个独立的Gauss随机过程：
{ xk=Fk−1xk−1+Γk−1wk−1 zk=Hkxk+vk { x k = F k − 1 x k − 1 + Γ k − 1 w k − 1 z k = H k x k + v k
其中：
xk是k时刻的系统状态向量，Fk是系统状态转移矩阵，而wk是过程演化噪声，Γk是噪声矩阵，zk是k时刻对系统的量测向量，Hk是量测矩阵，而vk是量测噪声。 x k 是 k 时 刻 的 系 统 状 态 向 量 ， F k 是 系 统 状 态 转 移 矩 阵 ， 而 w k 是 过 程 演 化 噪 声 ， Γ k 是 噪 声 矩 阵 ， z k 是 k 时 刻 对 系 统 的 量 测 向 量 ， H k 是 量 测 矩 阵 ， 而 v k 是 量 测 噪 声 。
{wk}是独立过程：wk∼N(0,Qwk) { w k } 是 独 立 过 程 ： w k ∼ N ( 0 , Q k w )
{vk}是独立过程：vk∼N(0,Qvk) { v k } 是 独 立 过 程 ： v k ∼ N ( 0 , Q k v )
系统初始状态：x0∼N(x⃗ 0),P0) 系 统 初 始 状 态 ： x 0 ∼ N ( x → 0 ) , P 0 )
卡尔曼滤波算法公式推到可见：最佳线性无偏估计BLUE
状态计算分成——–时间更新和滤波更新
时间更新： x^k|k−1=Fk−1x^k−1 x ^ k | k − 1 = F k − 1 x ^ k − 1
滤波更新： x^k=x^k|k−1+Gk|k−1×z~k|k−1 x ^ k = x ^ k | k − 1 + G k | k − 1 × z ~ k | k − 1
Gk|k−1=Pk|k−1HTk[HkPk|k−1HTk+Qvk]−1 G k | k − 1 = P k | k − 1 H k T [ H k P k | k − 1 H k T + Q k v ] − 1方差计算分成——-时间更新和滤波更新
时间更新： Pk|k−1=Fk−1Pk−1FTk−1+Γk−1Qwk−1ΓTk−1 P k | k − 1 = F k − 1 P k − 1 F k − 1 T + Γ k − 1 Q k − 1 w Γ k − 1 T
滤波更新： Pk=(I−Gk|k−1Hk)Pk|k−1 P k = ( I − G k | k − 1 H k ) P k | k − 1
1、 Kalman滤波理论的状态转移方程，就是离散时间系统的动态描述，又称为目标的动态模型，描述的是目标运动行为。
2、目标的运动行为描述主要用于对目标的跟踪滤波，因此又称为目标跟踪模型。
3、目标跟踪的数学模型
目标跟踪的主要目的就是估计移动目标的状态轨迹。虽然目标在空间上几乎从来不是一个真正的点，但通常还是把目标看作空间没有形状的一个点，特别对于目标建模更是如此。目标动态模型描述了目标状态又随时间的演化过程。
二、Kalman滤波器的应用
举例1
举例2
从题目中可得目标点在二维平面内移动，x轴方向状态变量为: (sx,vx,ax) ( s x , v x , a x ) ,y轴方向状态变量为: (sy,vy) ( s y , v y ) 所以整个目标状态可以用: x=(sx,vx,ax,sy,vy) x = ( s x , v x , a x , s y , v y ) 五维向量表示
初始状态为：
状态转移矩阵:
测量转移矩阵
不考虑飞行器内部扰动
所以 Γk=[00000] Γ k = [ 0 0 0 0 0 ]
先验状态初始协方差：
以上部分为自己设计，影响的是收敛速度。
测量噪声协方差：
matlab代码如下：
clear;
sigma = 10000; %测量误差
T = 2; %间隔时间步长
t1 = 400; %沿y轴匀速运动
t2 = 600; %向x轴方向转弯 a = 0.075
t3 = 610; %匀速运动
t4 = 660; %90°快转弯 a = -0.3
totaltime = 700; %总的观测时间为700秒 350步
V_y = -12;
ax = 0;
V_x = 0;
X_add = 0;
%初始值
f_k1 = [1 T T^2/2;0 1 T;0 0 1];
f_k2 = [1 T;0 1];
F_k = blkdiag(f_k1,f_k2); %预测状态转移矩阵
X_k1 = [1000 0 ax 800 -12]'; %状态初始值
Z_k = [1000;800];
H_k = [1 0 0 0 0;0 0 0 1 0]; %观测矩阵为2x5维
P_k1 = diag([sigma sigma/T sigma/T^2 sigma sigma/T]); %初始协方差矩阵
Qvk = sigma*eye(2); %观测噪声协方差
for i=1:450
if i>200 && i<=300
ax = 0.075;
X_k1(3,1) = ax;
end
if i>300 && i<=305
ax = 0;
X_k1(3,1) = ax;
end
if i>305 && i<=330
ax = -0.3;
X_k1(3,1) = ax;
end
if i>330
ax = 0;
X_k1(3,1) = ax;
end
X_add = X_add + V_x *T + 0.5*(ax*T^2); %x轴方向距离改变量
V_x = V_x + ax*T;
Z_k = [1000+X_add+wgn(1,1,40);800+V_y*T*(i - 1)+wgn(1,1,40)];
X_k2 = F_k*X_k1; %先验状态值
P_k2 = F_k*P_k1*F_k'; %先验协方差矩阵
Gk = P_k2*H_k'*inv(H_k*P_k2*H_k' + Qvk); %计算卡尔曼增益
P_k3 = (eye(5)-Gk*H_k)*P_k2; %后验协方差矩阵
X_k3 = X_k2 + Gk*(Z_k - H_k*X_k2);
P_k1 = P_k3;
X_k1 = X_k3;
X_plot(:,i) = X_k3;
Z_plot(:,i) = Z_k;
end
%画图
%plot(Z_plot(1,:),Z_plot(2,:));
%subplot(2,1,1);
plot(Z_plot(1,:),Z_plot(2,:),X_plot(1,:),X_plot(4,:),'black');
axis([0 6000 -10000 1000])
title('目标轨迹');
xlabel('X(米)');
ylabel('Y(米)');
legend('目标测量轨迹','目标真实轨迹');