卡尔曼滤波器 Python 卡尔曼滤波器matlab仿真

文章目录

• 0.引言
• 1.场景预设
• 2.卡尔曼滤波器
• 3.仿真及效果

0.引言

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

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

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

原系统方程为：

$\begin{cases} \dot{x}=Ax+Bu \\ y=Cx \end{cases}$

采用状态观测器的观测系统方程为：

$\begin{cases} \dot{\hat{x}}=A\hat{x}+Bu+K(y-\hat{y}) \\ \hat{y}=C\hat{x} \end{cases}$

为保证观测器的$\lim_{t\rightarrow\infty}(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{cases} \dot{x}=\begin{bmatrix} 0 &1\\0 & 0 \end{bmatrix}x+\begin{bmatrix} 0 \\ 1 \end{bmatrix}u\\[2ex] y=\begin{bmatrix} 0 & 1 \end{bmatrix}x \end{cases}$
选取采用间隔$dt$，则状态方程离散化后变为：
$\begin{cases} x(n)=\begin{bmatrix} 1 &dt \\ 0 & 1 \end{bmatrix}x(n-1)+\begin{bmatrix} 0 \\ dt \end{bmatrix}u(n-1)\\[2ex] y(n)=\begin{bmatrix} 0 & 1 \end{bmatrix}x(n) \end{cases}$
按照惯例定义
$A=\begin{bmatrix} 1 &dt \\ 0 & 1 \end{bmatrix},B=\begin{bmatrix} 0 \\ dt \end{bmatrix},C=\begin{bmatrix} 0 & 1 \end{bmatrix}, I=\begin{bmatrix} 1 &0 \\ 0 & 1 \end{bmatrix}$
由于系统具有一定的过程噪声$w$和测量噪声$v$。引入噪声的离散系统状态方程为：
$\begin{cases} x(n)=Ax(n-1)+B(u(n-1)+w$
为加以区别，使用$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$的先验估计，即$\hat{x}^{-}_k=A\hat{x}_{k-1}+Bu_k \\[2ex] P_k^-=AP_{k-1}A^T+Q$
3. 根据$x,P$的先验估计，求出$x$的后验估计$\hat{x}$，即$K_k=\frac{P_k^-C^T}{CP_k^-C^T+R}\\[2ex] \hat{x}_k=\hat{x}^-_k+K_k(y_v-C\hat{x}^-_k)\\[2ex] P_k=(I-K_kC)P_k^-$
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，测量协方差为$CPC^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=Bw$，而B的第一行为0，故$Q$的对角线第一元素必定为0.仿真的效果图如下：

实际值即状态方程的输出$y=Cx$，测量值即含噪声的输出$y_v=Cx+v$，估计值为卡尔曼滤波器对测量的最优估计值。