卡尔曼滤波原理及实例

卡尔曼滤波器又被称作“最佳线性滤波器”,它实现简单,在工程应用面上极广。而GNSS上面,卡尔曼滤波也有着两个主要应用: RTK算法与动态周跳探测。卡尔曼滤波来自于Rudof Emil Kalman在1960年发表的论文《A New Approach to Linear Filtering and Prediction Problmes》,此处是下载地址. 本文将以一个简单的例子来帮助理解卡尔曼波器的原理及实现。

一、卡尔曼滤波基本思想

用一句话说: 在已知预测值与观测值及其误差的条件下对物体状态的最优估计。在此处举个例子说明: 假设在一个恒温室,你有一个温度计,测量中误差为2度,在1点时,该温度计测量结果为10度,由于误差的存在,显然这个并不是恒温室的真实室温。问: 那么2点的时候该恒温室的温度是多少?

显然,根据1点时的观测值及条件,我们很容易推测2点时间温度为10度,中误差为2度,这两个可称为预测值预测值的误差,这个时候你使用温度计又测一下,11度,中误差依旧为2度,这个即为2点时刻的观测值观测值中误差。问: 那么这个时候恒温室的温度是多少呢?

由于预测值和观测值中误差相等,我们显然可以直接作个平均来作为恒温室的的温度: 10.5度。显然,这个温度的可信度(中误差)比单纯的预测值和观测值都应该是要小的,中误差变为\sqrt{2}度。这个过程就可以称作卡尔曼滤波了。这个过程又可以继续到下一个时刻: 在3点的时候你又使用温度计测量了温度,结果为10度,那么根据2点时刻所得的状态进行估计,可以进一步估计得到10.33度,误差为\sqrt{4/3}。如此往复,最终的结果将会越来越接近真实状态。

从上面过程可以归纳一下卡尔曼滤波两个主要过程:

1. 根据之前的状态预测当前状态
2. 根据观测值更新当前状态

这个涉及到这一个名词状态,指的是对当前事物的特征化描述,也是预测模型中的变量。

二、卡尔曼滤波及示例

2.1 预测模型建立

卡尔曼滤波是一个线性滤波器,应用卡尔曼滤波首先要考虑对系统进行建模。在此处我们可以认为系统模型如下:

x_t = F_t x_{t-1} + B_t u_t + w_t \quad \quad (1)

上式是什么意思呢?上式表示当前状态与上一状态之前推进关系,也就是预测模型,式中:

  • x_t: t时刻状态向量
  • u_t: t时刻的控制向量
  • F_t: t-1时刻到t时刻的状态转移矩阵
  • B_t: 控制向量对于状态向量的影响矩阵
  • w_t: 自然是预测结果状态向量的误差项

假设现在有一个匀速移动的小车,而我目前关注的状态有两个:小车的位置p和小车的速度v,则t时刻的状态向量为

x_t = \left[\begin{matrix} p_t\\ v_t  \end{matrix} \right]

而小车的速度模型显然如下:

p_t = p_{t-1} + v_{t-1} \times \Delta t   \\ v_t = v_t

上面两个公式可以将其写为矩阵形式:

\left[ \begin{matrix} p_t \\ v_t \end{matrix}\right]  = \left[ \begin{matrix} 1 & \Delta t \\ 0 &  1 \end{matrix}\right] \left[ \begin{matrix} p_{t-1} \\ v_{t-1} \end{matrix}\right]

显然,从式中我们就可以看出F_t的取值了,而B_tu_t由于是使用匀速模型,所以在此处忽略了。

由于状态向量必然存在误差,在此处我们将状态向量的误差矩阵记为P,是一个协方差矩阵。协方差矩阵的值会根据预测值与观测值的差异自动调整,所以其初始值可以自定义。而卡尔慢滤波同时也会对该矩阵进行预测:

P_t = F_t P_{t-1} F_t^T + Q_t \quad \quad (2)

这个里面值得注意就是转移矩阵误差项Q_t,和公式(1)的w_t直接相关。该项该如何理解呢?实际就是模型对于状态向量在预测过程中的影响,在上述小车模型中,如果确认小车是均速运动的话,那么Q的值可以定得非常小,而如果小车可能存在非匀速状态,那么Q就可以写得大一点。公式(2)表示了不确定性在各瞪眼时刻的传递关系。

根据公式(1)和(2)就可完成对于状态向量的大小和误差预测,设预测结果分别为x^-_t,P^-_t,则有:

x^-_t = F_t x_{t-1} + B_t u_t \\ P^-_t = F_t P_{t-1} F_t^T + Q_t

2.2 状态更新

现在有观测值向量z,其和状态向量之间存在线性关系:

z_t = H_t x_t + v_t

此处的v_t不是速度,而是观测值误差项,我们用协方差矩阵R来表示观测值z所带来的误差。然后就可以对预测值进行更新了:

x_t = x^-_t + K_t (z_t - H x^-_t)

其中是K_t叫作卡尔曼增益矩阵,定义如下:

K_t = P_t^- H^T(HP_t^- H^T +R )^{-1}

这个矩阵推导较为复杂,此处就不再说明。下面再对协方差阵再进行更新:

P_t = (I - K_t H)P_t^-

上述公式中的I为单位矩阵。

三、实例

现在假设能对小车进行位移观测,现使用卡尔慢滤波法最小车的状态进行估计。首先模拟位移观测值:

Z=(1:100);
noise=randn(1,100); %方差为1的高斯噪声
Z=Z+noise;

上面模拟100个观测值,然后对其拟加方差为1的噪声,显然,小车进行速度为1的匀速运行。然而设置初始条件及噪声设置:

X=[0;0];      % 初始状态矩阵(自定义)
P=[1 0; 0 1]; % 初始状态协方差矩阵(自定义)

F=[1 1; 0 1]; % 状态转移矩阵(模型决定)
Q=[0.0001,0; 0 0.0001]; % 状态转移协方差矩阵(模型决定)
H=[1 0];    %  观测矩阵(观测值决定)
R=1;        % 观测值噪声协方差阵(观测值决定)

由于我认为该小车是一定符合该模型的,所以的Q设置的非常小,而观测值由于只有一个,所以R值为实数。下面对其进行迭代绘图:

figure
hold on;
grid on;

for i=1:100
    X_ = F*X;   %观测值模型
    P_ = F*P*F' + Q;

    K = P_*H'/(H*P_*H'+R);   % 卡尔曼增益(Kalman Gain)
    X = X_ + K*(Z(i) - H*X_);
    P = (eye(2) - K*H)*P_;

    plot(X(1),X(2),'.');
end

运行结果如下:
Alt

四、扩展卡尔曼滤波

因为卡尔曼滤波器从一开始就是为线性系统设计的算法,不能用于非线性系统中。但是事实上多数系统都是非线性的,所以如果卡尔曼滤波器不能用在非线性系统中的话,那么它的应用范围就非常有限了。然而学者们对这个问题理解的非常深刻,而且也找到了解决方法,就是扩展卡尔曼滤波(EKF)。

实际上扩展卡尔曼滤波和卡尔曼滤波没有任何本质的区别,实现的方法也非常的类似,原因就是扩展卡尔曼滤波器使用了“线性化”的方法使得非线性系统变成线形系统,然后再使用卡尔曼滤波即可进行解决。如此来说,扩展卡尔曼滤波和卡尔曼滤波本身没有什么区别了么?最大的区别应该就是扩展卡尔曼滤波会发散,为何为会发散呢?实际上这是线性化所带来的后果。

线形化的过程实质上是线性化点处以近似的线性模型来代替非线性模形,所以如果离线性化点太远的话,这两个模型之间差异就会变大,从而使得卡尔曼滤波器效果变差。那么该如何对函数模形线性化呢?一般情况下使用全微分就行了,如下所示,假设存在t个非线性函数:
Z_1 = f_1(X_1,X_2,\cdots,X_n) \\ Z_2  = f_2(X_1,X_2,\cdots,X_n) \\ \cdots \cdots \\ Z_t = f_t(X_1,X_2,\cdots,X_n)
对其求全微分得
dZ_1 = \frac{\partial f_1}{\partial X_1} dX_1 + \frac{\partial f_1}{\partial X_2} dX_2  +\cdots + \frac{\partial f_1}{\partial X_n} dX_n\\ dZ_2 = \frac{\partial f_2}{\partial X_1} dX_1 + \frac{\partial f_2}{\partial X_2} dX_2  +\cdots + \frac{\partial f_2}{\partial X_n} dX_n \\ \cdots \cdots  \\ dZ_n = \frac{\partial f_n}{\partial X_1} dX_1 + \frac{\partial f_n}{\partial X_2} dX_2  +\cdots + \frac{\partial f_n}{\partial X_n} dX_n
然后再令X = (X_1,X_2,\cdots,X_n) 取得初值,将全微分的式子来代替非线形模形即可。显然,在此处,取得初值是比较重要的,如果初值不正确,误差将会较大。另外全微分实质上只是使用泰勒级数的一阶英作为近似值,精确度并不高,如果想要更好的精确度,可以取更高阶的结果,当然这也会导致线形模形复杂化。

那么扩展卡尔曼滤波有哪些地方需要线性化呢?主要有两个地方,一个地方是公式(1)的模型,另外一个是观测值向量与状态向量的关系。下面给出扩展卡尔曼滤波与卡尔曼滤波之间的对比:

Alt

图中已使用方框标明了两处需要线形化的地方,也是扩展卡尔曼滤波与卡尔曼滤波的不同之处。

此条目发表在GNSS分类目录,贴了标签。将固定链接加入收藏夹。

发表评论

电子邮件地址不会被公开。