机器人机构分析与综合课件：卡尔曼滤波

可以看出，因为温度计的covariance比较小（比 较相信温度计），所以估算出的最优温度值偏向温 度计的值。
现在我们已经得到k时刻的最优温度值了，下一步 就是要进入k+1时刻，进行新的最优估算。到现在 为止，好我们还要算出k时刻那个最优值 （24.56度）的偏差。算法如下：((1Kg)*5^2)^0.5=2.35。这里的5就是上面的k时 刻预测的那个23度温度值的偏差，得出的2.35就 是进入k+1时刻以后k时刻估算出的最优温度值的
xk A
B、uk
Zk H wk vk
系统状态 系统矩阵 状态的控制量 观测值 观测矩阵 过程噪声 测量噪声
实际温度 温度变化转移 （通常没有） 温度计读数 摄氏度-〉华氏度 温度变化偏差 读数误差
KF Model - Definition
定义
为 先验状态估计， 为
后验状态估计值
先验误差和后验误差定义如下：
头脸识别 图像分割 图像边缘检测
Temperature Problem - Ideal World
假设当前室内温度仅跟上一时刻有 关
温度计观测（摄氏－〉华氏） 根据连续的观测值来推算实际温度
变化
Temperature Problem - Real World
假设当前室内温度仅跟上一时刻有 关
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);
Signal Processing
数字滤波：通过一种算法排除可能的随机 干扰，提高检测精度的一种手段
线性系统 f(A+B) = f(A) + f(B) 数学方法处理 噪声信号输入 --〉尽可能少噪声输出
Use for
机器人导航、控制 传感器数据融合 雷达系统以及导弹追踪 计算机图像处理
Analysis – Time & Measure Update
测量更新
时间更新
Result - Estimation
Result - Error
北京地区1989年11月～12月
95年12月至96年4月侯平均温度实况与预估对照图 a.北京 b. 福州 c.成都
Application
Matlab 程序
但变化中可能有噪声
温度计观测（摄氏－〉华氏）
读数会有误差
两种噪声相互无关 根据连续的观测值来推算实际温度
变化
假如我们要估算k时刻的实际温度值。首先要根据 k-1时刻的温度值，来预测k时刻的温度。
因为相信温度是恒定的，所以会得到k时刻的温度 预测值是跟 k-1时刻一样的，假设是23度，同时 该值的高斯噪声的偏差是5度.
维纳滤波：使用全部观测值保证平稳性
Kalman Filtering - Advantages
卡尔曼滤波器是一个“optimal recursive data processing algorithm （最优化自回归数据处理算法）”
对于解决很大部分的问题，他是最优，效 率最高甚至是最有用的
Formula of KF
偏差（对应于上面的3）。
Kalman Filtering – First Sight
KF是根据上一状态的估计值和当前状态的 观测值推出当前状态的估计值的滤波方法
S(t) = f ( S(t-1) , O(t) )
它是用状态方程和递推方法进行估计的， 因而卡尔曼滤波对信号的平稳性和时不变 性不做要求
协方差：
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为高斯白噪声
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
卡尔曼滤波
The Kalman Filtering
Rudolf Emil Kalman
匈牙利数学家 BS&MS at MIT PhD at Columbia 1960年发表的论文《A New Approach
to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新 方法）
然后，从温度计那里得到了k时刻的温度值，假设 是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值 ，分别是23度和25度。究竟实际温度是多少呢？ 相信自己还是相信温度计呢？究竟相信谁多一点， 我们可以用他们的 covariance来判断。
因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78 ，我们可以估算出k时刻的实际温度值是： 23+0.78* (25-23)=24.56度。