卡尔曼滤波学习

在求学的道路上还是一定要有求知和专注的精神，之前是涉猎的比较多，导致现在面广而不实，只能从头开始学习，近期在学习卡尔曼滤波，久闻其名，却不知道如何应用。这次根据看到的几篇博客，来自己记录一下，增强记忆，也便于各位朋友来指点：

一、什么是卡尔曼滤波器

 卡尔曼滤波器，是一种“optimal recursive data processing algorithm”方法，最优化自回归数据处理算法，在机器人导航、

控制、传感器信号融合以及导弹等方面得到了广泛应用，近年来在图像处理方面又得到了广泛应用。而我要学习它，则是因为近

段时间以来，接触到定位导航方面的事情。

二、卡尔曼滤波器算法

 首先我们引入一个线性离散控制过程的系统：

 X（K）=A*X（K-1）+B*U(K)+W(K)

 Z(K)=HX(K)+V(K)

上式中，X(K)是系统当前的状态，U(K)是K时刻系统的控制量，Z(K)是K时刻系统的测量值，A、B是系统参数，H是测量系统参数

W和V分别表示过程系统和测量系统的噪声，服从高斯分布，协方差分别为Q和R。

卡尔曼滤波器主要包含以下五个公式：

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)

 上式中，P(K|K-1)表示X(K|K-1)对应的协方差，P(K-1|K-1)代表X(K-1|K-1)对应的协方差。

X(K|K)=X(K|K-1)+Kg(K)*(Z(K)-H*X(K|K-1))..............................................(3)

上式中，X(K|K)表示由预测值和测量值得到的当前时刻的最优估计值，Kg(K)代表卡尔曼增益。

Kg(K)=P(K|K-1)*H'/[H*P(K|K-1)H'+R]........................................................(4)

 通过式（3）已经得到了当前状态的最优估计，但是为了使得系统能够不断迭代，需要更新得到X(K|K)对应的协方差：

 P(K|K)=(I-Kg(K)*H)*P(K|K-1).........................................................................(5)

至此，卡尔曼滤波器的算法已经完毕，网上还有很多温度或小车的例子，此处不再赘述。

三、Matlab实现

******************************************************************************

转载自：https://www.cnblogs.com/sillykog/p/6618587.html

*****************************************************************************

对比四条曲线，会发现跟随性能不一样，是由于曲线值太小，噪声相对太大的缘故，将值变大，噪声相对就小了，跟随就好了

%第一条曲线

%t=0.1:0.1:6;

%X=t;

%第二条曲线

%t=1:1:60;

%X=t;

%第三条曲线

%t=1:1:60;

%X=sin(t);

%第四条曲线

t=1:1:60;

X=60* sin (t);

　　

 

N=1:60;

plot (N,X, 'r' );

hold  on;

 

   

%系统方程：X(k+1)=A*X(k)+w(k) 

%观测方程：Z(k)=H*Z(k)+v(k) 

%这个是生成高斯噪声的随机数

w= randn (1,60);   

v= randn (1,60);

 

A=1;

H=1;

X_k(1)=0;                       %状态估计初值             

P_kk(1)=0;                      %P（k/k）

P_k(1)=0;                       %P（k/k-1） 这个初始化不需要，就给你们看看变量的对应

Z_k(1)=X_k(1)+w(1) ;   　　      %测量值

 

R=( std (v)).^2; 

Q=( std (w)).^2; 

 

Kg(1)=P_kk(1)*H '/(H*P_kk(1)*H' +R);         %卡尔曼增益 Kg

P_k(1)=A*P_kk(1)*A'+Q ;                    %方差预测   P_k/k-1

 

for  i =2:60

     X_k( i )=A*X_k( i -1)+Kg( i -1)*(Z_k( i -1)-H*A*X_k( i -1));  %这边就直接代入式（1），所以没出现

     Kg( i )=P_k( i -1)*H '\((H*P_k(i-1)*H' +R)) ;

     P_k( i )=A*P_kk( i -1)*A'+Q;

     P_kk( i )=P_k( i -1)-Kg( i )*H*P_kk( i -1) ;

     Z_k( i )=H*X( i )+w( i ) ;

end 

n=1:60;

plot (n,X_k);

**************************************************************************************************

转载自：http://blog.chinaunix.net/uid-26694208-id-3184442.html?page=3

********************************************************************************************************

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');

函数先抄上，尚未验证，等装了matlab再回来补充验证结果。