在求学的道路上还是一定要有求知和专注的精神，之前是涉猎的比较多，导致现在面广而不实，只能从头开始学习，近期在学习卡尔曼滤波，久闻其名，却不知道如何应用。这次根据看到的几篇博客，来自己记录一下，增强记忆，也便于各位朋友来指点：
一、什么是卡尔曼滤波器
卡尔曼滤波器，是一种“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实现
******************************************************************************
*****************************************************************************
对比四条曲线，会发现跟随性能不一样，是由于曲线值太小，噪声相对太大的缘故，将值变大，噪声相对就小了，跟随就好了
%第一条曲线
%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);
**************************************************************************************************
********************************************************************************************************
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再回来补充验证结果。
