卡尔曼滤波的理解以及参数调整

一、前言

卡尔曼滤波器是一种最优线性状态估计方法（等价于“在最小均方误差准则下的最佳线性滤波器”），所谓状态估计就是通过数学方法寻求与观测数据最佳拟合的状态向量。

在移动机器人导航方面，卡尔曼滤波是最常用的状态估计方法。直观上来讲，卡尔曼滤波器在这里起了数据融合的作用，只需要输入当前的测量值（多个传感器数据）和上一个周期的估计值就能估计当前的状态，这个估计出来的当前状态综合考量了传感器数据（即所谓的观察值、测量值）和上一状态的数据，为当前最优估计，可以认为这个估计出来的值是最可靠的值。由于我们在SLAM中主要用它做位置估计，所以前面所谓的估计值就是估计位置坐标了，而输入的传感器数据包括码盘推算的位置、陀螺仪的角速度等（当然可以有多个陀螺仪和码盘），最后输出的最优估计用来作为机器人的当前位置被导航算法以外的其他程序所调用。

列举一下卡尔曼滤波的优点：采用递归方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能够进行状态估计，不需要大量的存储空间，每一步的计算量小，计算步骤清晰，非常适合计算机处理。

二、问题描述与定义

1、前提

首先明确卡尔曼滤波器的前提假设：

• 信息过程的足够精确的模型，是由白噪声所激发的线性、离散和有限维动态系统（可以是时变的）；
• 每次测量信号都包含着附加的白噪声分量。

满足上述条件就可以使用卡尔曼滤波器。

2、问题描述

定义一个随机离散时间过程的状态向量 ，该过程用一个离散随机差分方程描述：
$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$$

其中n维向量$x_k$为k时刻的系统状态变量，n维向量$x_{k-1}$ 是k-1时刻的系统状态变量。A是状态转移矩阵或者过程增益矩阵，是$n\times n$阶方阵，它将k-1时刻状态和当前的k时刻状态联系起来。B是可选的控制输入 $u\in R^l$的增益，在大多数实际情况下并没有控制增益，所以 $B{u}_{k-1}$这一项很愉快的变成零了。 $w_{k-1}$是n维向量，代表过程激励噪声，它对应了$x_k$ 中每个分量的噪声，是期望为0，协方差为Q的高斯白噪声，$w_k\sim N(0,Q)$ 。
再定义一个观测变量 ，得到观测方程：
$$z_k=H{x}_k+v_k$$

其中观测值$z_k$是m阶向量，状态变量$x_k$ 是n阶向量。H是$m\times n$ 阶矩阵，代表状态变量$x_k$ 对测量变量 $z_k$的增益。观测噪声$v_k$ 是期望为0，协方差为R的高斯白噪声，$v_k\sim N(0,R)$。

3、举例

如果对上面两个公式不太明白，我举一个例子说明：
在目标跟踪的应用中，假设质点坐标为$x,y$是直接观测得到的，质点在x、y轴方向速度分别为$v_x$,$v_y$，那么系统状态变量 $x_k=(x,y,v_x,v_y{)}^T$，系统观测变量 $z_k=(\bar{x},\bar{y}{)}^T$ ，系统没有控制输入，所以状态方程就成了：
$$x_k=A{x}_{k_1}+w_{k-1}$$
它的状态转移矩阵A根据运动学公式确定：
$$\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

其实用矩阵的形式写开上面的方程就很好理解了：
$$x_k=A{x}_{k-1}+w_{k-1}⟶{\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]}_k=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]}_{k-1}+w_{k-1}={\left[\begin{array}{c}x+\Delta t\cdot v_x\\ y+\Delta t\cdot v_y\\ v_x\\ v_y\end{array}\right]}_{k-1}+w_{k-1}$$

• 这里简要说一下状态转移矩阵A的确定：

（1） 在标量卡尔曼滤波中，比如测量值是温度、湿度，一般认为下一个时刻该温度或者湿度维持不变，这种情况下状态转移矩阵通常就是标量1。
（2） 在导航和目标跟踪中卡尔曼滤波常被用来做位置估计，比如匀速直线运动（CV）和匀加速直线运动（CA）：
$$A_{CV}=\left[\begin{array}{ccc}1& T& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],A_{CA}=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

在前面的举例中x,y方向上的运动就被微分近似为匀速直线运动。
（3） 若被估计的过程或观测变量与过程的关系是非线性的，此时不能够直接应用卡尔曼滤波（因为不满足上一节提到的卡尔曼滤波适用的前提必须是线性系统），这个时候扩展卡尔曼滤波（EKF）应运而生，它用雅克比矩阵将期望和方差线性化，从而将卡尔曼滤波扩展到非线性系统，但是EKF由于考虑了泰勒级数的展开，运算量大大增加。
该过程的观测方程为
$$z_k=H{x}_k+v_k$$
它的观测矩阵H也是要指定的，它的目的是将m维的测量值转换到n维与状态变量相对应，由于直接观测的量是位置$z_k=(\bar{x},\bar{y}{)}^T$ ，我们只需要取状态变量的前两个元素就够了，所以H设计成如下：
$$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$
用矩阵形式写开就是
$$z_k=H{x}_k+v_k⟶{\left[\begin{array}{c}\bar{x}\\ \bar{y}\end{array}\right]}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]+v_k={\left[\begin{array}{c}x\\ y\end{array}\right]}_k+v_k$$

• 我再举一个简单例子：

一个运动目标的状态变量$x_k=(x(k),\dot{x}(k),\ddot{x}(x){)}^T$,其中第一项是目标在k时刻的位置，这里假设为一维坐标，第二项为k时刻的速度，第三项为k时刻的加速度，雷达仅能观测到目标的位置即$x(k)$ ,那么它的状态转移矩阵就该设计成：
$$A=\left[\begin{array}{ccc}1& \Delta t& \frac{\Delta t^2}{2}\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$

它的观测矩阵就该设计成：
$$H=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]$$

到了这里怎么设计矩阵A和H应该有思路了吧。

三、算法流程

接下来就开始介绍卡尔曼滤波最核心的五个更新方程了

1、预测：
$${\hat{x}}_{\bar{k}}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
$${\hat{P}}_{\bar{k}}=A{\hat{P}}_{k-1}+Q$$
2、更新：
$${\hat{x}}_k={\hat{x}}_{\bar{k}}+K_k(z_k-H{\hat{x}}_{\bar{k}})$$
$$K_k=\frac{{\hat{P}}_{\bar{k}}{H}^T}{H{\hat{P}}_{\bar{k}}{H}^T+R}$$
$${\hat{P}}_k=(I-K_{k}H){\hat{P}}_{\bar{k}}$$
先来解释一下公式中各个变量的含义：
${\hat{x}}_{\bar{k}}$：表示k时刻先验状态估计值，这是算法根据前次迭代结果（就是上一次循环的后验估计值）做出的不可靠估计。

${\hat{x}}_k,{\hat{x}}_{k-1}$：分别表示k时刻、k-1时刻后验状态估计值，也就是要输出的该时刻最优估计值，这个值是卡尔曼滤波的结果。

$A$：表示状态转移矩阵，是$n\times n$ 阶方阵，它是算法对状态变量进行预测的依据，状态转移矩阵如果不符合目标模型有可能导致滤波发散，它的确定请参看第二节中的举例。

$B$：表示可选的控制输入$u\in R^l$的增益，在大多数实际情况下并没有控制增益。

$u_{k-1}$：表示k-1时刻的控制增益，一般没有这个变量，可以设为0。

${\hat{P}}_{\bar{k}}$：表示k时刻的先验估计协方差，这个协方差矩阵只要确定了一开始的${\hat{P}}_0$，后面都可以递推出来，而且初始协方差矩阵${\hat{P}}_0$ 只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。

${\hat{P}}_k、{\hat{P}}_{k-1}$：分别表示k时刻、k-1时刻的后验估计协方差，是滤波结果之一。

$Q$：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；二是在自适应卡尔曼滤波（AKF）中Q矩阵是随时间变化的。

$K_k$：表示卡尔曼增益，是滤波的中间结果。

$z_k$：表示测量值，是m阶向量。

$H$：表示量测矩阵，是$m\times n$ 阶矩阵，它把m维测量值转换到n维与状态变量相对应。

$R$：表示测量噪声协方差，它是一个数值，这是和仪器相关的一个特性，作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。

四、算法实现
以下是一段卡尔曼滤波与平滑滤波的对比

clear
clc;
N=300;
CON = 25;%房间温度，假定温度是恒定的
%%%%%%%%%%%%%%%卡尔曼滤波%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,N);
y = 2^0.5 * randn(1,N) + CON;%加过程噪声的测量值

x(1) = 1;
p = 10;

Q = cov(randn(1,N));%过程噪声协方差
R = cov(randn(1,N));%观测噪声协方差
for k = 2 : N
x(k) = x(k - 1);%预估计k时刻状态变量的值
p = p + Q;%对应于预估值的协方差，由于状态变量是标量，状态转移矩阵A为数值1
kg = p / (p + R);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
end


%%%%%%%%%%%平滑滤波%%%%%%%%%%%%%%%%%%%%%%%%

Filter_Wid = 10;
smooth_res = zeros(1,N);
for i = Filter_Wid + 1 : N
tempsum = 0;
for j = i - Filter_Wid : i - 1
tempsum = tempsum + y(j);
end
smooth_res(i) = tempsum / Filter_Wid;
end
% figure(1);
% hist(y);
t=1:N;
figure(1);
expValue = zeros(1,N);
for i = 1: N
expValue(i) = CON;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');
legend('real temperature','kalman result','measured value','smooth result');
axis([0 N 20 30])
xlabel('Sample Time');
ylabel('Room Temperature');
title('Smooth Filter VS Kalman Filter');

结果如下图：

五、引用
[1] 王学斌, 徐建宏, 张章. 卡尔曼滤波器参数分析与应用方法研究[J]. 计算机应用与软件, 2012, 29(6):212-215.