Kalman滤波

来源：互联网发布：手机直播美颜软件编辑：程序博客网时间：2024/05/21 00:18

Kalman滤波中文又称卡尔曼滤波（不知道谁翻译的），上网一搜，一大堆公式，搞的我头昏脑胀，公式稍后奉上，先说一说这东西是干啥的吧。试着直接表述了一下他是干啥的，发现说不明白啊！不如这样，先说点别的引入一下。

大家都知道，我们所测量的任何数据都是存在误差的，这个应该没有疑问吧！无论你是用多么精准的仪器，在物理上有句话叫做误差不可避免，但我们可以使其尽可能的小。有点这个意思啦，卡尔曼滤波在某一层面上的意思应该就是使误差尽可能的小。那么他是怎么做到的呢？举个例子：

小明正值青春发育阶段，个子长得特别快，假设小明在前一天的个子高是K，那么我们需要做的就是根据前一天小明的身高估计出今天小明的身高，一般我们一天也长不了多高，所以我们估计今天小明的身高也是K，我们从上一天中得到了一个最优的偏差，设为X，现在先不解释X是怎么来的，一会我会用红色字体标注一下X是怎么来的。今天我们用尺子量小明的身高设为Q，因为我们估计的今天的身高和用尺子测量的身高都是存在误差的，所以我们设一个不确定值为L，意思是我们估计身高在K±L，尺子量的身高也是Q±L，好了，有了这些数据，我们开始计算小明今天究竟多高吧！

今天小明的身高：

1.求高斯噪声偏差：G=√（X^2+L^2）

2.令kg=√(G^2/(G^2+L^2))

3.实际身高H=X+kg*(Q - X)

今天小明的身高我们已经知道了，那么来看看明天的吧！

同样我们估计明天小明的个子高还是K，最优偏差是X，这里X=√((1 - kg)*G^2)，这回知道X是怎么来的了吧！好了，明天求得过程和今天的是一样的，我就是想让大家知道X是怎么来的。。。

下面介绍一下kalman滤波的5个公式吧！（这是在别的博客抄来的）

我认为如果只是想用kalman滤波知道这些就够了，如果想更深入推荐两个博客，我也是在这两篇博客学到了很多，都是大神级别的，只能膜拜一下！

大神一博客：http://blog.chinaunix.net/uid-26694208-id-3184442.html

大神二博客：http://blog.csdn.net/xiahouzuoxin/article/details/39582483

这里附一个MATLAB程序：

function [zx,zy]=kalmanf(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y)
%----------------输入参数--------------------------------------------------
% A -- 系统矩阵
% H -- 观察矩阵
% Rw -- 扰动向量
% Rv -- 测量噪声
% Rw_c -- 扰动向量的协方差
% Rv_c -- 测量噪声的协方差
% x0 -- 节点初始位置向量(x,y)'
% p0 -- 初试协方差阵
% y -- 采样周期
%--------------------------------------------------------------------------

%--------------------------------------------------------------------------
% X(k) = A*X(k) + Rw(k) 噪声Q
% Z(k) = H*X(k) + Rv(k) 噪声R
% x(k|k-1) = A*x(k-1|k-1) + B*U(k)
% P(k|k-1) = A*P(k-1|k-1)*A' + Q
% x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
% kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
% P(k|k) = (I-kg(k)*H)*P(k|k-1)
%--------------------------------------------------------------------------

len = length( y ); %获取采样点数
r = size(A,1); %获取系统矩阵A的行数
I= eye( r(1) ); %生成单位矩阵
P1 = zeros( r(1),r(1) ); %初始化协方差阵
%初始化节点位置，协方差阵
X = x0;
P = p0;
len1 = size( H*X ,1);
for s=1:1:len
z1 = A*X+ Rw; % X(k) = A*X(k) + Rw(k) 协方差 Rw_c
zx(1:r(1),s) = z1(1:r(1)) ;
z2 = H*X + Rv; % Z(k) = H*X(k) + Rv(k) 协方差 Rv_c
zy(1:len1,s) = z2(1:len1 );

P1 = A*P*A' + Rw_c; % P(k|k-1) = A*P(k-1|k-1)*A' + Q
K = P1*H'*inv( H*P1*H' + Rv_c ); % kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
X = A*X + K*( zy(1:len1,s) - H*A*X );
% x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
P = ( I - K*H ) * P1; % P(k|k) = (I-kg(k)*H)*P(k|k-1)
end
return

1 0