鲁道夫·卡尔曼(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('目标测量轨迹','目标真实轨迹');