卡尔曼滤波 代码实现 Python

一、卡尔曼滤波理论和概念

1.预备概念

（1）滤波

我们在模电数电中学的滤波、滤波器（Filter）等概念和这里的卡尔曼滤波可以说本质概念是统一的，都是去除或者减少信号（状态）中的干扰量，最大可能得得到想要的部分，逼近真实值。滤波器是为了筛选出来希望得到的频率信号，这里的卡尔曼滤波也是为了减少噪声的干扰，是测量信号更接近实际信号。

（2）噪声

无用信号也叫作噪声，在通信领域中，某些频段的信号我们不想要，对于我们就是噪声；在传感器测量数据时候，无用信号表现为幅度上的干扰。
噪声的种类取决于其功率谱密度，相关知识在通信原理中有详细介绍。如果噪声的幅值服从正态分布，频谱密度又是均匀分布，那么我们便称之为高斯白噪声。后面的研究我们认为，传感器的测量噪声都属于高斯白噪声。
按照噪声引入的阶段、来源不同，我们主要把噪声分成过程噪声和观测噪声。
举个例子，现在我们测量一个房间的温度。房间温度有一个客观存在的数值25度，不依赖于我们的测量工具而存在。现在我用温度计测出来温度24.5度，好一点的温度计可能测出来24.9度。显然这一类干扰是由于温度计（测量工具）引起的，且几乎没有能总是完全测准的工具，这一类干扰就属于我们说的观测噪声。
如何知道观测噪声的大小呢？我们一般用统计的方法，长期测，多测几次，谁的观测噪声大谁的小就容易知道了。
过程噪声体现在过程二字，显然是对于一个时刻到下一个时刻而言的。继续上面的例子，我房间的温度一分钟前后测量应该是不变的，但是考虑到光照、空气流通等因素，前后测量结果也会被干扰。
过程噪声一般通过对比试验的方法得到。

（3）动态系统的观测和描述

基于状态空间的方法，我们可以这样描述和观测一个离散系统。

$X(k)=AX(k-1)+GW(k)\\ Y(k)=HX(K)+V(k)\\ W~N(0,Q);V~N(0,R)$
式中k是离散的时间，A是状态转移矩阵，X(k)是k时刻的状态变量，W(k)是过程噪声（结合上面对过程噪声的直观理解，的确应该出现在状态转移方程中。）Y是测量值，H是观测矩阵，是从状态空间到测量空间的映射，V是观测噪声。

二、卡尔曼滤波的应用

1、主要流程

• 状态初始估计值 $X(0|0)$,初始估计量协方差矩阵 $P(0|0)$;
• 状态预测值 $X(k+1|k)=AX(k|k)$;
• 测量值 Y，以及由于预测值和直接测量值的不一致而引起的残差
  $\epsilon(k+1)=Y(k+1)-HX(k+1|k)$;
• 预测协方差矩阵 $P(k+1|k)=AP(k|k)A^T+GQG^T$(预测自然和第一个状态转移方程有关)
• 卡尔曼增益矩阵 $K(k+1)=P(k+1|k)H^T[HP(k+1|k)H^T+R]^{-1}$
• 最优状态估计值 $X(k+1|k+1)=X(k+1|k)+K(k+1)\epsilon(k+1)$
• 更新(估计)协方差矩阵 $P(k+1)=[I_n-K(k+1)H]P(k+1|k)$
• 下一个循环，始于$X(k+1|k+1),P(k+1|k+1)$

2、一维状态——温度测量实例

(1)问题描述

现在要研究一个房间中的温度。假设房间温度理论值应该是25度（没有任何干扰，分子热运动稳定，热传导稳定）。但是收到实际情况下，空气流通、光变化的影响，真实的实际温度在波动，这个干扰就是过程噪声$W,W-N(0,Q=0.001)$。如果Q=0，那么这个温度就恒定不变了。这就是Q的意义。
同时,不失一般性地，设$A=1,G=1$。
现在用某牌温度计测量室温，假设温度计比较劣质，噪声$V-N(0,R=0.25)$

（2）建模与滤波

按照 二-1 的流程进行建模和求解

• 假设初始的估计值就是23.9度，就是直接的测量值，实际值是24度（我们不知道），初始的估计值协方差矩阵为0.01；
• 下一个时刻，测量值是24.5度，真实温度是24.1度（我们不知道），偏差0.4度；我们实际能得到的是23.9度（上一个时刻的估计值）和24.1度，当前测量值。
• 先求预测值、测量值、残差；
• 再预测协方差矩阵、卡尔曼增益矩阵；
• 修正预测值就可以得到估计值

（3）matlab代码

%卡尔曼滤波在一维温度测量中的应用
N=120;%采样点的个数
T=25;%房间温度的理论值，假设不受干扰的话。
X_expect=T*ones(1,N);%每个采样点如果没有干扰的话也应该是25度
X=zeros(1,N);%保存各个采样点的真实温度
X_est=zeros(1,N);%各个时刻的估计值，有两类来源，初始的估计值是直接给出来的，后面的估计值是经过卡尔曼滤波得到的，是最接近真实值的；
X_pre=0;%各个时刻的预测值，也就是根据前面已经有的状态，结合状态转移矩阵得到的预测值。
Z=zeros(1,N);%各个时刻的测量值，也就是传感器给出的。
P_pre=0;%协方差矩阵有两类，一个是预测状态的协方差矩阵，另一个是估计状态的协方差矩阵，两者具有互推、递推关系。
P_est=zeros(1,N);%维数1

%初始化
X(1)=25.1;%初始的真实温度
P_est(1)=0.01;%初始值的协方差
Z(1)=24.9;%初始的测量值
X_est(1)=Z(1);%初始的最好值，估计值，其实不太好，直接用了初始测量值。

%噪声
Q=0.01;%白噪声的方差,过程噪声，加入到了状态转移的过程，均值为0，方差为Q，衡量噪声的大小。
W=sqrt(Q)*randn(1,N);
R=0.25;%测量噪声的方差，均值为0
V=sqrt(R)*randn(1,N);

%系统矩阵
A=1;%状态转移矩阵，理论方程应该是各个时刻温度恒定的；
G=1;%噪声驱动矩阵
H=1;%观测矩阵，即从状态空间到观测空间的映射。
E=eye(1);%系统1维

%模拟测量过程并滤波

for k=2:N
    X(k)=A*X(k-1)+G*W(k-1);%真实温度要考虑过程噪声
    %测量
    Z(k)=H*X(k)+V(k);
    
    %预测状态
    X_pre=A*X_est(k-1);%依靠上一次的最优估计值
    %预测状态的协方差矩阵
    P_pre=A*P_est(k-1)*A'+Q;%过程噪声自然是干预到预测状态的协方差矩阵中
    
    K=P_pre/(H*P_pre*H'+R);%卡尔曼增益求解
    e=Z(k)-H*X_pre;%观测与预测的残差
    X_est(k)=X_pre+e*K;%最优估计值
    P_est(k)=(E-K*H)*P_pre;
  
end

%误差计算：
er_meas=zeros(1,N);
er_kalm=zeros(1,N);

for k=1:N
    er_meas(k)=abs(Z(k)-X(k));%测量值和实际值的真实误差
    er_kalm(k)=abs(X_est(k)-X(k));
end

%数据可视化
t=1:N;   %同时画出('无干扰的理论值','直接测量值','实际值','kalman估计值')
figure
plot(t,X_expect,'-b',t,Z,'-r.',t,X,'-ko',t,X_est,'-g*');
legend('无干扰的理论值','直接测量值','实际值','kalman估计值');
xlabel('采样时间');
ylabel('温度值/℃');

figure
plot(t,er_meas,'-b.',t,er_kalm,'-k*');
xlabel('采样时间');
ylabel('温度偏差');
legend('测量偏差','滤波值偏差');

我也不知道为什么不高亮555

运行结果

3、二维状态——目标跟踪

总结

卡尔曼滤波是什么？它做了什么？我们有很多角度来理解它。
（1）在卡尔曼增益的加权下，卡尔曼滤波得到的信号的估计值与真实值均方误差达到最小的。推导卡尔曼增益的求导，卡尔曼增益就是增益的一个极值点。
（2）根据 一（3） 部分方程的描述，得到一个系统的状态可以有两种方法，第一种对应第一个方程，是状态转移的方法，结合上一时刻的状态得到下一个时刻的预测状态；第二个方法对应第二个方程，是使用传感器直接观测的方法。显然，第一种方法得到的结果是连续的，总是依赖于上一个时刻，第二种方法得到的结果是不连续的，是当前时刻直接测出来的。两种方法都有各自的不确定性（过程噪声和观测噪声）。卡尔曼滤波是将这两种方法结合起来，利用预测值和观测值，得到最优的估计值。对于两者权重的选择就是基于其不确定度，或者说方差。
（3）卡尔曼滤波是将系统的估计值看成噪声作用到线性系统下系统状态值。