# 卡尔曼滤波的原理说明

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是从视频中提取的目标坐标值，一个四十个坐标。

• 本文已收录于以下专栏：

## Kalman filter卡尔曼滤波的原理说明

• kuikuijia
• 2015年04月29日 20:10
• 1461

## 卡尔曼滤波的原理说明

• qq1041256623
• 2014年03月22日 19:53
• 534

## 详解卡尔曼滤波原理

• u010720661
• 2017年03月18日 13:54
• 2506

## 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.4

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维...
• sinat_20265495
• 2016年11月23日 16:15
• 1134

## 卡尔曼滤波原理及实现

• lybaihu
• 2017年02月09日 09:13
• 6607

## 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明：Kalman滤波用于温度测量的实例 ...
• sinat_20265495
• 2016年11月23日 14:48
• 2482

## 卡尔曼滤波原理说明

• dinglijay
• 2014年04月10日 13:57
• 340

## 卡尔曼滤波原理二：扩展卡尔曼

1、理论部分       上一篇介绍了线性卡尔曼滤波器，当系统为线性高斯模型时，滤波器能给出最优的估计，但是实际系统总是存在不同程度的非线性，如平方、三角关系、开方等。对于非线性系统，可以采用的一种...
• u012936940
• 2017年08月16日 16:18
• 3461

## 【转载】卡尔曼滤波的原理说明

• liyuzhiwly
• 2015年11月23日 13:45
• 242

## 卡尔曼滤波的原理简单说明

1. 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf ...
• u013780605
• 2016年10月14日 18:10
• 175

举报原因： 您举报文章：卡尔曼滤波的原理说明 色情 政治 抄袭 广告 招聘 骂人 其他 (最多只允许输入30个字)