卡尔曼滤波原理及应用（一）

出于科研需要，打算开始学习卡尔曼滤波（Kalmam Filter）。很早之前就听说过卡尔曼滤波,但一直没能深入学习，这次终于有机会了，哈哈。

1.卡尔曼滤波的发展过程

卡尔曼滤波的本质属于"估计"范畴.先介绍下估计，所谓“估计”问题，就是对受到随机干扰和随机测量误差作用的物理系统，按照某种性能指标为最优的原则，从具有随机误差的测量数据中提取信息，估计出系统的某些参数状态变量。最早的估计方法是大名鼎鼎的数学家高斯于1795年发明的最小二乘法(,)。最小二乘法计算简单，易于应用，但没有考虑被估参数和观察数据的统计特性，因次该方法不是最优估计。之后，1912年，英国统计学家费尔希提出最大似然法，从概率密度角度来考虑估计问题。对于随机过程的估计，1940年，控制论的创始人维纳根据火力控制系统上的需要提出一种在频域中设计统计最有滤波器的方法，该方法被称为Wiener滤波。同时期，苏联数学家柯尔莫戈洛夫提出并初次解决广义离散平稳随机序列的预测和外推问题。由于维纳滤波采用频域设计法，运算复杂，解析困难等问题，促使科学家寻求在时域直接设计最优滤波器的方法，于是，卡尔曼滤波应运而生！（发明人：匈牙利裔美国数学家鲁道夫·卡尔曼，1930—）

卡尔曼滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量少，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。由于卡尔曼滤波理论具有上述强大优点，它作为一种最重要的最优估计理论被广泛应用于各领域，如惯性制导、全球定位系统、目标跟踪系统、通信与信号处理、金融等等。

2.经典卡尔曼滤波器的原理简介

经典卡尔曼于1960年提出，此后数十年，不断衍生发展，本文讨论的是经典卡尔曼滤波。卡尔曼滤波器的推导过程设计大量繁琐的数学公式，就不在此推导。略去数学公式，把握其中的基本前提、基本假设和基本方程，我们同样能领略到卡尔曼的魅力。

(1)基本前提：卡尔曼滤波应用对象为模型精确和随机干扰信号统计特性已知的线性系统

(2)基本方程： 

                       

式中，k为离散时间，系统在时刻k的状态为X(k),Y(k)为对应状态的观测信号，W(k)为输入的高斯白噪声，V(k)为观测噪声。称式(1)为状态方程，式(2)为观测方程。称为状态转移矩阵，为噪声驱动矩阵，H为观测矩阵。

说明的是，上述方程中涉及到的参数X为卡尔曼估计值，Y为实际观测值。、和H均为和系统相关的模型参数，所以当系统模型足够准确，卡尔曼滤波才能正常工作；卡尔曼滤波认为观测值和真实值有误差，这个误差的来源有两部分，即过程噪声W和观测噪声V。 什么是过程噪声呢？过程噪声就是外界对于系统的干扰，例如：为温度测量系统中，过程噪声是由于人体干扰、阳光照射、风等因素造成的；测量噪声于传感器测量精度息息相关，即由传感器的测量误差引起。测量噪声和过程噪声的特性均可由统计得到。

(3)基本假设：假设1：W和V是均值为零，方差各为Q和R的不相关白噪声；

                       假设2：初始状态X(0)不相关于W和V

总结：卡尔曼滤波问题可归结为：基于观测信号{},求状态X(k)的线性方差估计值

 

3.算法设计

经过推导，卡尔曼滤波的递推算法如下：

状态的一步预测： 

状态更新：            

                               

滤波增益矩阵：     

一步预测协方差阵：

协方差阵更新：       

 其中，,R是观测噪声方差，Q是过程噪声方差。

 

4.例程演示

卡尔曼滤波在温度测量中的应用：假设我们要研究的对象为一个房间的温度。根据经验判断，这个房间的温度大概在25℃左右，可能受空气流通、阳光等因素影响，房间内温度会小幅度地波动，采样周期为1分钟，利用卡尔曼滤波来实时对温度进行最优估计。

源码如下：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能说明：卡尔曼滤波用在一维温度数据测量系统中
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;
close all;
N=120;%采样点个数，时间单位是分钟
constant=25;%室内温度的理论值，受过程噪声的干扰，该值会有波动
%对状态和测量初始化
Xexcept=constant*ones(1,N);%房间各时刻真是温度值
Xkf=zeros(1,N);%卡尔曼滤波处理的状态，也叫估计值
Z=zeros(1,N);%温度测量值
P=zeros(1,N);
%赋初值
X(1)=25.1;%假设房间温度为25.1℃
P(1)=0.01;%初始值的协方差
Z(1)=24.9;
Xkf(1)=Z(1);%初始测量值为24.9℃，可作为滤波器的初始估计状态
%噪声
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N);%方差决定噪声大小
V=sqrt(R)*randn(1,N);%方差决定噪声大小
%系数矩阵
F=1;
G=1;
H=1;
I=eye(1);%本系统状态为一维
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模拟房间温度和测量过程并滤波
for k=2:N
    %第一步：随时间推移，房间真实温度波动变化
    %k时刻房间真实温度，对于温度传感器来说，这个真实值是不知道的
    X(k)=F*X(k-1)+G*W(k-1);
    %第二步：随时间推移，获取真实数据
    %温度传感器对k时刻房间的测量
    %Kalman滤波是站在温度传感器角度进行的，它不知道此刻真实状态X(k)，只能利用上
    %次测量值Z(k)和上次估计值Xkf(k)来做处理，其目的是最大限度地降低测量噪声R的
    %干扰，尽可能地逼近X(k),这也是Kalman滤波的本质目的
    Z(k)=H*X(k)+V(k);
    %第三步：Kalman滤波
    %有了k时刻的观测值Z(k)和k-1时刻的状态，就可以进行滤波过程
    X_pre=F*Xkf(k-1);              %状态预测；
    P_pre=F*P(k-1)*F'+Q;           %协方差预测
    Kg=P_pre*inv(H*P_pre*H'+R);    %计算kalman增益
    e=Z(k)-H*X_pre;                %新息
    Xkf(k)=X_pre+Kg*e;             %状态更新
    P(k)=(I-Kg*H)*P_pre;           %协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算误差
Err_Measure=zeros(1,N);%测量值与真实值之间的偏差
Err_Kalman=zeros(1,N); %估计值与真实值之间的偏差
for k=1:N
    Err_Measure(k)=abs(Z(k)-X(k));
    Err_Kalman(k)=abs(Xkf(k)-X(k));
end
t=1:N;
figure%画图显示
%依次输出理论值，叠加过程噪声（受波动影响）的真实值，温度传感器和Kalman估计值
plot(t,Xexcept,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','Kalman滤波值');
xlabel('采样时间/s');
ylabel('温度/℃');
figure;%画图显示
plot(t,Err_Measure,'-b',t,Err_Kalman,'-K*');
legend('测量偏差','Kalman滤波偏差');
xlabel('采样时间/s');
ylabel('温度偏差/℃');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

效果如下：

                   

结束。初次学习，难免理解有误，各位高手请多指教。