卡尔曼滤波 The Kalman Filtering article 2005.11

Contents 背景简介 一个实际问题 算法描述 实验应用

Rudolf Emil Kalman 匈牙利数学家 BS&MS at MIT PhD at Columbia 1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）

Signal Processing 数字滤波：通过一种算法排除可能的随机干扰，提高检测精度的一种手段 线性系统 f(A+B) = f(A) + f(B) 数学方法处理 噪声信号输入-〉尽可能少噪声输出

Use for 机器人导航、控制 传感器数据融合 雷达系统以及导弹追踪 计算机图像处理 头脸识别 图像分割 图像边缘检测

Temperature Problem - Ideal World 假设当前室内温度仅跟上一时刻有关 温度计观测（摄氏－〉华氏） 根据连续的观测值来推算实际温度变化

Temperature Problem - Real World 假设当前室内温度仅跟上一时刻有关 但变化中可能有噪声 温度计观测（摄氏－〉华氏） 读数会有误差 两种噪声相互无关 根据连续的观测值来推算实际温度变化

Kalman Filtering – First Sight KF是根据上一状态的估计值和当前状态的观测值推出当前状态的估计值的滤波方法 S(t) = f ( S(t-1) , O(t) ) 它是用状态方程和递推方法进行估计的，因而卡尔曼滤波对信号的平稳性和时不变性不做要求 维纳滤波：使用全部观测值保证平稳性

Kalman Filtering - Advantages 卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）” 对于解决很大部分的问题，他是最优，效率最高甚至是最有用的

Formula of KF xk 系统状态 实际温度 A 系统矩阵 温度变化转移 B、uk 状态的控制量 （通常没有） Zk 观测值 温度计读数 H 观测矩阵 摄氏度-〉华氏度 wk 过程噪声 温度变化偏差 vk 测量噪声 读数误差

KF Model - Definition 定义 为 先验状态估计， 为 后验状态估计值 先验误差和后验误差定义如下： 协方差：

KF Model - Algorithm 递推公式 如果没有误差，可以认为 则包含全部误差的信息，称为新息(innovation) K为修正矩阵，或称混合因子 (Blend factor)

Blend factor Matrix 修正矩阵的形式有多种，其中一种为： R->0 => K = 1/H

Discrete KF

Flow Chart 任意给定初值均可，但P!=0

Experiment 目标： 用KF估计一个常数（电压） 约束： 数据本身有误差（电压不稳） 观测有误差（电压表不准）

Analysis – Matrix Assignment A=1，B=0，H=1 简化为： w,v为高斯白噪声

Analysis – Time & Measure Update 测量更新 时间更新

Result - Estimation

Result - Error

Application 视频跟踪

Matlab 程序 clear; N=10; p=[8 0 0 ;0 10 0;0 0 5]; pp=zeros(3,3); ppp=zeros(3,3); x=[0 0 0.2]'; xx=zeros(3,1); xxx=zeros(3,1); k=zeros(3,1); Rn=0.15; A=[1 2 2;0 1 2;0 0 1]; H=[1 0 0]; Z=[0.36 1.56 3.64 6.44 10 15 21 25.2 32.2 40.4]; I=[1 0 0 ;0 1 0 ; 0 0 1] ; for t=1:N; pp=A* p*A'; k= pp*H'*inv(H* pp*H'+ Rn); ppp=(I- k*H)* pp; xx=A*x+ k*(Z(t)-H*A* x); xxx=A*xx; disp('状态一步预测值'); disp(xxx); disp('状态滤波值'); disp(xx); disp('滤波均方误差阵'); disp(ppp); p=ppp; x=xx; end

t=0.1:0.1:6;; x=exp(t); i=1:60; plot(i,x,'r'); hold on %%以上是要预测的曲线（或者说是要跟踪的曲线）：y=exp(x); %系统方程：x(k+1)=fi*x(k)+gm*w(k) %观测方程：z(k)=h*x(k)+v(k) fi=1.1052; h=1; gm=1; w=randn(1,60); v=randn(1,60); xy(1)=0; p(1)=0; z(1)=x(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:60 xy(i)=fi*xy(i-1)+k(i-1)*(z(i-1)-xy(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); %p(i)=fi*p(i-1)*fi'-fi*p(i-1)*h'*inv((p(i-1)+R))*h*p(i-1)*fi+Q;%%%% %k(i)=fi*p(i)*h'*inv(h*p(i)*h'+R); z(i)=x(i)+w(i); end n=1:60; plot(n,xy);