# 卡尔曼滤波的原理说明

226人阅读 评论(0)

2．卡尔曼滤波器的介绍
（Introduction to the Kalman Filter）

3． 卡尔曼滤波器算法
（The Kalman Filter Algorithm）

X(k)=A X(k-1)+B U(k)+W(k)

Z(k)=H X(k)+V(k)

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

4． 简单例子
（A Simple Example）

X(k|k-1)=X(k-1|k-1) ……….. (6)

P(k|k-1)=P(k-1|k-1) +Q ……… (7)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)

××××××××××××××××××

clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end

V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;

p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
close all;
clc;
n=40;

current_point=point.point;

plot(current_point(:,1),current_point(:,2),'r');

%系统方程：x(k+1)=fi*x(k)+gm*w(k)
%观测方程：z(k)=h*x(k)+v(k)

x=current_point(:,1);
y=current_point(:,2);

fi=1;
h=1;
gm=5;

w=randn(1,n);
v=randn(1,n);

xy_x(1)=x(1);
xy_y(1)=y(1);

p(1)=0;
z_x(1)=x(1)+w(1);
z_y(1)=y(1)+w(1);
R=(std(v)).^2;
Q=(std(w)).^2;
k(1)=fi*p(1)*h'*inv(h*p(1)*h'+R);
pp(1)=fi*p(1)*fi'+gm*Q*gm';

for i=2:n
xy_x(i)=fi*xy_x(i-1)+k(i-1)*(z_x(i-1)-xy_x(i-1));
xy_y(i)=fi*xy_y(i-1)+k(i-1)*(z_y(i-1)-xy_y(i-1));

k(i)=pp(i-1)*h'*inv((h*pp(i-1)*h'+R));
pp(i)=fi*p(i-1)*fi'+gm*Q*gm';
p(i)=pp(i-1)-k(i)*h*p(i-1);

z_x(i)=x(i)+w(i);
z_y(i)=y(i)+w(i);
end

hold on;
plot(xy_x,xy_y);
grid on;

point.mat是从视频中提取的目标坐标值，一个四十个坐标。

0
0

* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
个人资料
• 访问：138059次
• 积分：1914
• 等级：
• 排名：千里之外
• 原创：60篇
• 转载：120篇
• 译文：3篇
• 评论：13条
阅读排行
评论排行
最新评论