卡尔曼滤波_附代码

卡尔曼滤波

卡尔曼滤波以其发明者鲁道夫.E.卡尔曼（Rudolph E. Kalman）命名，但根据文献可知卡尔曼并非第一个提出该算法的人，Peter Swerling在更早之前就提出了类似算法¹。

卡尔曼滤波的典型实例是从一组有限、包含噪声的观测序列中预测出物体的位置和速度。最为常见的卡尔曼滤波器是锁相环。

线性卡尔曼滤波

待估计模型

卡尔曼滤波建立在隐马尔可夫模型上，其基本动态系统使用一个马尔可夫链表示。

     

使用公式表示即，对于需要求解的状态        满足

其中        为        状态的转移矩阵，在现实的过程中他可能是时变的，但是在这里假设其为常量。       矩阵        是可能存在的控制器        的输入-控制模型。对于这一隐马尔可夫链有观测值        满足

其中        矩阵        为观测模型，在真实情况中观测模型可能会是时变的，但在此处假设其为常量。

随机变量        分别为过程噪声和观测噪声，假设他们是独立的高斯白噪声且

推导

卡尔曼滤波器是建立在贝叶斯滤波器的基础上的²，首先介绍贝叶斯滤波器。

贝叶斯准则与全概率公式

贝叶斯准则：

全概率公式：

贝叶斯滤波器

贝叶斯滤波器的思想即使用已有的观测值和控制输入来估计当前时刻的状态，对于状态        ，观测值        ，控制量       ，写成概率的形式即：

根据贝叶斯准则

分母与        无关，使用常数        代替，由于隐马尔可夫链的性质，某时刻的观测值只与其状态有关，则   上式可以化简为：

可以理解为使用先验和似然估计后验。则先验为：

代入全概率公式，由于积分中乘积的前一项为根据前一时刻状态预测下一时刻状态，根据马尔可夫链的性质，这一概率与观测值        无关。同时，马尔可夫性质表明只有当前的控制信号和上一时刻的状态对当前时刻的状态有影响，则积分乘积前一项中的        可以被化为        ，乘积后一项中的        可以被化为       。同时观察可知乘积的第二项即       ：

后验为：

代入待估计模型

求解先验分布

由于假设噪声符合高斯分布，则        也应符合高斯分布，设其均值为        ，方差        。对于先验公式，有

则        可以表示成  的形式，其中        为常数，有

两个高斯分布的乘积        也为高斯分布，而        是关于        的二次型，则求解         可以得到该分布的均值       ，        为其方差的倒数      。可以解得：

为了求解积分，需要使用一些技巧来简化运算，定义二次型函数

可以发现        为        的二次型，且有  ，则可以将        拆分来去积分。定义

则

由于        为关于        的二次型，且        符合高斯分布，则使用求一阶导数和二阶导数的方法来求取其方差        和均值       。

一阶导数：

令一阶导数等于零

则

二阶导数：

则得到先验分布       

求解后验分布

由系统的观测方程可以得到  ，已经求得先验分布       ，则后验分布

可以简化为        ，其中

由于        为        的二次型，同时        符合高斯分布，继续通过求解一阶导数和二阶导数的方法求均值和方差。

一阶导数：

令一阶导数等于零

令

化简        可以得到一个更常见的形式：

则

二阶导数：

将        代入        的表达式中，可以得到更加简化的表达形式

结论

至此，卡尔曼滤波器的五条公式全部推出，分别为式（1~5），即：

可以看到，卡尔曼滤波器即是在贝叶斯滤波器的基础上加入了带有噪声的模型。观察        可以发现

即当观测误差趋于 0 时，引入观测值进行修正的权重将增大；当预测误差趋于 0 时，引入观测值进行修正的权重将减小并趋于 0。

卡尔曼滤波器的实现

卡尔曼滤波器通过如下的算法进行实现：

     

接下来在倒立摆模型中实现卡尔曼滤波器，倒立摆模型预测方程近似如下：

其中        为小车位移、杆角度、小车速度、杆角速度，       为小车重量、杆重量，       为杆长度，       为向小车施加的力，       在离散系统中为两次更新间经过的时间，。

在实现中一般不希望或不能观测所有的变量，而且在观测中会引入误差，假设只有小车速度和杆角速度能被测量，观测方程如下：

使用卡尔曼滤波器实现状态估计。使用 Open AI 的开源库 gym 中的CartPole-v1 进行仿真，使用 PID 算法进行控制，由于本文只关心卡尔曼滤波，PID参数较为粗放。仿真结果如下图：

卡尔曼滤波器的 python 代码如下：

class KFilter:
    def __init__(self, f_mat, b_mat, q_mat, h_mat, r_mat):
        self.f_mat = f_mat
        self.b_mat = b_mat
        self.q_mat = q_mat
        self.h_mat = h_mat
        self.r_mat = r_mat

    def kal_filter(self, x_mat, p_mat, z_mat, action):
        x_predict = self.f_mat * x_mat + self.b_mat * action
        p_predict = self.f_mat * p_mat * self.f_mat.T + self.q_mat
        k_num = p_predict * self.h_mat.T * np.linalg.pinv(self.h_mat * p_predict * self.h_mat.T + self.r_mat)
        x_mat = x_predict + k_num * (z_mat - self.h_mat * x_predict)
        p_mat = (np.eye(4) - k_num * self.h_mat) * p_predict
        return x_mat, p_mat

倒立摆的 PID 算法修改自 GitHub ³

参考文献

由于***文章无法插入外链，以下引用均未附原文链接

[1] Wikipedia

[2] 卡尔曼滤波器的数学推导—木独—CSDN

[3] The PID control implementation of cart-pole environment in gym—lukewys—Github