【MATLAB】卡尔曼滤波器的原理及仿真（初学者专用）

0.引言

\qquad 本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档（Kalman Filter）。官方教程的B站链接如下，在此对分享资源的Up主表示感谢。(如不能正常播放或需要看中文字幕，请点击此处B站链接)

【官方教程】卡尔曼滤波器教程与MATLAB仿真（全)(中英字幕)

另外提供友情链接如下：

1. 卡尔曼滤波算法（知乎，多维情形）
2. Matlab关于传感器融合知识的介绍
3. 扩展卡尔曼滤波算法（CSDN，适合入门）
4. 扩展卡尔曼滤波算法（知乎，适合入门）
5. 扩展卡尔曼滤波算法（知乎，适合进阶）

\qquad 本文不会完全照搬视频中的所有内容，只会介绍有关卡尔曼滤波器关于定位方面的知识。卡尔曼滤波器除最原始的版本（KF）外，其延伸版本主要有三种——扩展卡尔曼滤波器（EKF）、无迹卡尔曼滤波器（UKF）、粒子滤波器（PF）。它们的运算复杂度依次递增，其中KF、EKF、UKF是建立在随机噪声是高斯分布的基础上的；PF理论则没有此预设。它们的关系如下图所示：

\qquad 首先，KF是一种状态观测器。状态观测器是针对可观系统，根据输出$y$和输入$u$对系统内部状态$u$进行观测的结构单元。其构图如下（参考视频链接）：

原系统方程为：
$$\{\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx\end{array}$$
采用状态观测器的观测系统方程为：
$$\{\begin{array}{l}\dot{\hat{x}}=A\hat{x}+Bu+K(y-\hat{y})\\ \hat{y}=C\hat{x}\end{array}$$
为保证观测器的$$\underset{t\to \infty }{\lim }(x-\hat{x}{)}^T(x-\hat{x})=0$$
需要$A-KC$是负定的。而卡尔曼滤波器就是一种状态观测器，只不过它是随机系统的状态观测器，其结构框图如下：

在输入$u$和动态系统Plant中间会引入过程噪声$w$，而在输出$y$（即测量）和实际测量$y_v$中间会引入传感器噪声$v$,而卡尔曼滤波器则是根据$u,y_v$求得测量的最优估计$y_e$。

1.场景预设

\qquad 假设一辆汽车在做匀速直线运动（一维场景），驾驶员通过油门控制了汽车的加速度恒定。忽略汽车所受的阻力、质量变化，并假设驾驶员操作会给汽车的牵引力造成一定的过程噪声。选择汽车的位移和速度为状态变量$x=[p,v{]}^T$，则状态变量导数为汽车的速度和加速度即$\dot{x}=[v,a{]}^T$，选取控制变量$u=a$，测量量为汽车的位移。则状态方程如下：
$$\{\begin{array}{l}\dot{x}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]x+\left[\begin{array}{c}0\\ 1\end{array}\right]u\\ y=\left[\begin{array}{cc}0& 1\end{array}\right]x\end{array}$$
选取采用间隔$dt$，则状态方程离散化后变为：
$$\{\begin{array}{l}x(n)=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right]x(n-1)+\left[\begin{array}{c}0\\ dt\end{array}\right]u(n-1)\\ y(n)=\left[\begin{array}{cc}0& 1\end{array}\right]x(n)\end{array}$$
按照惯例定义
$$A=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}0\\ dt\end{array}\right],C=\left[\begin{array}{cc}0& 1\end{array}\right],I=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$
由于系统具有一定的过程噪声$w$和测量噪声$v$。引入噪声的离散系统状态方程为：
$$\{\begin{array}{l}x(n)=Ax(n-1)+B(u(n-1)+w^{\prime })=Bu(n)+w\\ y_v(n)=Cx(n)+v\end{array}$$
为加以区别，使用$y_v$在本文中表示含噪声的真实测量。定义随机噪声的方差：
$E((w-\overline{w}{)}^T(w-\overline{w}))=Q,E((v-\overline{v}{)}^T(v-\overline{v}))=R$
汽车的初始真实位移为0.2，真实速度为0即$x_0=[0.2,0{]}^T$.

2.卡尔曼滤波器

\qquad 为方便初学者入门，本文不会从贝叶斯估计的角度证明KF的公式，但会将其应用步骤以尽可能简单的手段列出。

1. 需要设定一个初始的状态估计值即${\hat{x}}_0$，需要认为规定初始的先验估计的协方差矩阵$P_0=E((x-\hat{x}{)}^T(x-\hat{x}))$。${\hat{x}}_0$一般是凭经验设定的，如果系统偏离平衡状态不远设定$[0,0{]}^T$也无妨；$P_0$在教学视频中并未给出设定方式，一般设定为$Q$即可。令$k=1$，仿真开始。
2. 获取当前测量量$y_v$，先求出$x,P$的先验估计，即$$\begin{gathered} {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k \\ {P}_k^{-}=A{P}_{k-1}{A}^T+Q \end{gathered}$$
3. 根据$x,P$的先验估计，求出$x$的后验估计$\hat{x}$，即$$\begin{gathered} K_k=\frac{P_k^{-}{C}^T}{C{P}_k^{-}{C}^T+R} \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_v-C{\hat{x}}_k^{-}) \\ {P}_k=(I-K_{k}C)P_k^{-} \end{gathered}$$
4. 根据$\hat{y}=C\hat{x}$求出测量的卡尔曼估计值$\hat{y}$.
5. $k=k+1$,转步2

\qquad 严格地来说，如果步骤4要成立，需要$E((w-\overline{w}{)}^T(x-\overline{x}))=0$即$w$和$v$下相互独立，且系统方程中直接传输矩阵$D=0$，但考虑到本文是写给初学者的，所以在此默认了两个条件是成立的。
\qquad 状态协方差即为P，测量协方差为$CP{C}^T$。卡尔曼滤波器算法收敛一般是指的是测量协方差收敛。

3.仿真及效果

在此附上Matlab的仿真代码

% 模拟要求汽车在一维空间的加速和减速过程
% 控制变量u是汽车的加速度
% 状态变量x=[p,v],x^hat=[v,a]
% w为控制变量的随机扰动，v为测量的随机扰动
% Q为w的方差，R为v的方差，假设w与v相互独立
clear
dt = 0.1;  % 采样间隔
N = 100;  % 仿真数
Q = diag([0,0.05]); R = 1;
A = [1,dt;
     0,1];
B = [0;
     dt];
C = [1,0];
P = Q;
I = eye(2);
x0 = [0.2;0];  % 初始位置和速度
xh0 = [0;0];  % x0的估计
u = ones(1,N);  % 加速度恒定
w = sqrt(Q)*randn(2,N); % 控制变量的误差2*N
v = sqrt(R)*randn(1,N); % 测量误差1*N
ye_list = zeros(size(u));  % 估计值
yv_list = zeros(size(u));  % 测量值
y_list = zeros(size(u));  % 实际值
cov_list = zeros(size(u));  % 测量方差
for i = 1:numel(u)
    xreal = A*x0 + B*u(i);  % 真实的状态变量
    yreal = C*x0;  % 真实的测量
    x1 = xreal + w(:,i);  % 含噪声的状态变量
    yv = yreal + v(i);  % 含噪声的测量
    xfe = A*xh0 + B*u(i);  % 先验的状态变量
    Pfe = A*P*A'+ Q;  % 先验的状态变量协方差
    K = Pfe*C'/(C*Pfe*C'+R);  % 卡尔曼最优增益
    xh1 = xfe + K*(yv-C*xfe);  % 当前的状态估计
    ye = C*xh1;
    P = (I-K*C)*Pfe;
    x0 = x1;
    xh0 = xh1;
    y_list(i) = yreal;
    yv_list(i) = yv;
    ye_list(i) = ye;
    cov_list(i) = C*P*C';
end
ax = (1:N).*dt;
figure(1);
subplot(2,2,1)
plot(ax,y_list,ax,yv_list,ax,ye_list)
legend('实际','测量','估计','Location','best')
title('汽车的位移')
ylabel('位移/m')
xlabel('时间/s')
subplot(2,2,2)
plot(ax,yv_list-y_list,ax,ye_list-y_list)
legend('测量','估计','Location','best')
title('汽车的位移误差')
ylabel('位移/m')
xlabel('时间/s')
subplot(2,2,[3,4])
plot(ax,cov_list)
legend('测量方差','Location','best')
title('测量方差')
ylabel('方差/m^2')
xlabel('时间/s')

本文设定的采样间隔$dt=0.1，x_0=[0.2,0{]}^T,{\hat{x}}_0=[0,0{]}^T$，注意由于$w=B{w}^{\prime }$，而B的第一行为0，故$Q$的对角线第一元素必定为0.仿真的效果图如下：

实际值即状态方程的输出$y=Cx$，测量值即含噪声的输出$y_v=Cx+v$，估计值为卡尔曼滤波器对测量的最优估计值。
\qquad 希望本文对您有所帮助，谢谢阅读！如有异议，欢迎评论区指正！

另外对本话题感兴趣的读者可以继续阅读有关适用于非线性系统的扩展卡尔曼滤波算法的介绍。