传感数据分析——自适应Kalman滤波

发布时间：2024年01月05日

传感数据分析——自适应Kalman滤波

文章目录

前言

前期文章已叙述在传感数据中基于滑动窗口的中值滤波和均值滤波、一阶滞后滤波、EMA滤波和卡尔曼滤波，并基于Python实现。
本期将叙述自适应卡尔曼滤波及Python的实现。

卡尔曼滤波属于数据融合的递归方法，本质是状态估计，即根据当前观测值和物理模型估计当前时刻的状态值，滤波方法如下：
（1）向前推算状态变量
X ∧ k + 1 ∣ k = A X ∧ k ∣ k + B u k \begin{equation}\overset{\wedge}{\mathbf{X}}_{k+1|k}=\mathbf{A}\overset{\wedge}{\mathbf{X}}_{k|k}+\mathbf{B}\mathbf{u}_k\end{equation} X∧k+1∣k?=AX∧k∣k?+Buk???
（2）向前推算误差协方差
P ? ∧ k + 1 ∣ k = A P ? ∧ k ∣ k A T + Q \begin{equation}\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k+1|k}=\mathbf{A}\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k|k}\mathbf{A}^T+\mathbf{Q}\end{equation} P∧k+1∣k?=AP∧k∣k?AT+Q??
（3）计算卡尔曼增益
K k + 1 = P ? ∧ k + 1 ∣ k H T ( H ? ∧ P ? ∧ k + 1 ∣ k H T + R ) ? 1 \begin{equation}\mathbf{K}_{k+1}=\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k+1\mid k}\mathbf{H}^T(\overset{\wedge}{\operatorname*{\mathbf{H}}}\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k+1\mid k}\mathbf{H}^T+\mathbf{R})^{-1}\end{equation} Kk+1?=P∧k+1∣k?HT(H∧P∧k+1∣k?HT+R)?1??
（4）由观测变量更新估计值
X ? ∧ k + 1 ∣ k + 1 = X ? ∧ k + 1 ∣ k + K ? k + 1 ( z ? ∧ k + 1 ? H ? ∧ X ? ∧ k + 1 ∣ k ) \begin{equation}\overset{\wedge}{\operatorname*{\mathbf{X}}}_{k+1|k+1}=\overset{\wedge}{\operatorname*{\mathbf{X}}}_{k+1|k}+\overset{}{\operatorname*{\mathbf{K}}}_{k+1}(\overset{\wedge}{\operatorname*{\mathbf{z}}}_{k+1}-\overset{\wedge}{\operatorname*{\mathbf{H}}}\overset{\wedge}{\operatorname*{\mathbf{X}}}_{k+1|k})\end{equation} X∧k+1∣k+1?=X∧k+1∣k?+Kk+1?(z∧k+1??H∧X∧k+1∣k?)??
（5）更新误差协方差
P ? ∧ k + 1 ∣ k + 1 = ( I ? K k + 1 H ) P ? ∧ k + 1 ∣ k \begin{equation}\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k+1|k+1}=(\mathbf{I}-\mathbf{K}_{k+1}\mathbf{H})\overset{\wedge}{\operatorname*{\mathbf{P}}}_{k+1|k}\end{equation} P∧k+1∣k+1?=(I?Kk+1?H)P∧k+1∣k???

为实现 Q Q Q和 R R R的自适应调节，故采用公式：
Q k = α Q k ? 1 + ( 1 ? α ) ( K k ε k ε k T K k T ) \begin{equation}Q_{k}=\alpha Q_{k-1}+(1-\alpha)({K}_{k}\varepsilon_{k}\varepsilon_{k}^{T}{K}_{k}^{T})\end{equation} Qk?=αQk?1?+(1?α)(Kk?εk?εkT?KkT?)??
R k = α R k ? 1 + ( 1 ? α ) ( ε k ε k T + H k P k ? H k T ) \begin{equation}R_{k}=\alpha R_{k-1}+(1-\alpha)(\varepsilon_{k}\varepsilon_{k}^{T}+H_{k}^{}P_{k}^{-}H_{k}^{T})\end{equation} Rk?=αRk?1?+(1?α)(εk?εkT?+Hk?Pk??HkT?)??
此处 ε k \varepsilon_{k} εk?为：
ε k = ( z ? ∧ k ? H ? ∧ X ? ∧ k ∣ k ? 1 ) \begin{equation}\varepsilon_{k}=(\overset{\wedge}{\operatorname*{\mathbf{z}}}_{k}-\overset{\wedge}{\operatorname*{\mathbf{H}}}\overset{\wedge}{\operatorname*{\mathbf{X}}}_{k|k-1}) \end{equation} εk?=(z∧k??H∧X∧k∣k?1?)??
具体公式推导可参考论文《Adaptive Adjustment of Noise Covariance in Kalman Filter for Dynamic State Estimation》

本文正文内容

一、运行环境

系统： Windows 10 / Ubuntu 20.04
编程语言： Python 3.8
文本编译器： Vscode
所需库：matplotlib >= 2.2.2 , numpy >= 1.19.5

二、Python实现

代码如下（示例）：

# @copyright all reseved
# @author: Persist_Zhang
def AdaptiveKalmanFilter(Z_mea, X_last=0, P_last=0, Q_last=0.0018, R_last=0.00542):
        """
        一维数据的自适应卡尔曼滤波
        :param Z_mea:  测量数据
        :param X_last: 上一时刻的状态
        :param P_last: 上一时刻的协方差
        :param Q_last: 上一时刻的系统噪声方差
        :param R_last: 上一时刻的测量噪声方差
        :return:
        """
        # 状态预测
        X_pre = F * X_last
        # 协方差预测
        P_pre = F * P_last * F + Q
        # Kalman增益计算 
        Kg = P_pre * H / (H * P_pre * H + R)
        # 状态更新
        Re = (Z_mea - X_pre)
        X_post = X_pre + Kg * Re
        # 协方差更新
        P_post = (1 - Kg * H) * P_pre
        # 系统噪声与测量噪声的自适应更新
        R_post = alpha * R_last + (1 - alpha) * (Re * Re + P_pre)
        Q_post = alpha * Q_last + (1 - alpha) * (Kg * Re * Re * Kg)
        # t -> t + 1
        P_last = P_post
        X_last = X_post
        Q_last = Q_post
        R_last = R_post
        
        return X_post, P_last, X_last, Q_last, R_last


def kf_process(data):
    x_last = data[0]
    p_last = 0
    akf_pred = []
    q_last = Q
    r_last = R
    for i in range(len(data)):
        pred, p_last, x_last, q_last, r_last = AdaptiveKalmanFilter(data[i], x_last, p_last, q_last, r_last)
        akf_pred.append(pred)
    return np.array(akf_pred)



if __name__ == '__main__':
    # 导入数据
    data= np.random.rand(1000)
    # 保存路径
    save_path = './result/'
    # 测试数据

    data_filter = kf_process(data)
    # 绘图并保存
    plt.figure(figsize=(16,9))
    plt.plot(data)
    plt.plot(data_filter)
    plt.legend(['Original', 'Adaptive Kalman Filter'])
    plt.title("Adaptive Kalman Filter Test", fontsize=18)
    plt.savefig(save_path + "Adaptive Kalman-Filter_test.png")
    plt.show()

结果图
自适应卡尔曼滤波

由图可知，蓝色的曲线表示原始数据，橙色的曲线为滤波后的值。可以发现，原数据噪声较多，自适应Kalman滤波后平稳很多，滤波效果相较于传统Kalman滤波效果好很多。

总结

以上就是本节对传感数据进行自适应Kalman滤波算法的内容，实现了对Kalman滤波中 Q Q Q和 R R R的自适应调节，并通过Python的实现，后续还会持续更新本系列，本系列收藏过100，后台联系我，我会免费将全部相关的代码都发给你。

文章来源:https://blog.csdn.net/weixin_39753819/article/details/135374425
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。
最新文章