快速上手的Python版二维卡尔曼滤波解析

这篇具有很好参考价值的文章主要介绍了快速上手的Python版二维卡尔曼滤波解析。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

卡尔曼滤波是最好的线性滤波,但是需要推导的公式教多,也很细,这里推荐一个B站博主视频讲解的关于卡尔曼滤波,讲的很好,很细,适合小白学习,链接地址为:添加链接描述。如果完全没接触过卡尔曼滤波的,建议从第一集开始学习。
下面是我跟着这位博主学习后,再加上其他大神写的代码,融入我自己的理解,对代码进行修改后的版本,每一个部分都有详细的注释,更加的通俗易懂,希望能帮助到需要快速上手卡尔曼滤波的学习者。

卡尔曼的本质就下面五个公式:
快速上手的Python版二维卡尔曼滤波解析
参数说明,见下:
快速上手的Python版二维卡尔曼滤波解析

在卡尔曼滤波中需要调的参数主要有两个:
(1)Q矩阵:过程的协方差矩阵。注意:过程的协方差越小,说明预估的更准确,更可信。

(2)R矩阵:测量的协方差矩阵。注意:测量的协方差矩阵越小,说明测量的结果更准确,更可信。
快速上手的Python版二维卡尔曼滤波解析
怎么理解呢,假设你的测量设备是几万元或几千元的,测量值的可靠度较高(测量值的权值占比就需要较大),那么你的R矩阵就定义的小一些,比如定义为[[0.1,0],[0,0.1],这时候Q矩阵就可以定义的大一些,比如定义为[[1,0],[0,1]]。

如果你是使用的测量设备是某宝9.9元包邮的,那么你就应该相信估计值(估计值的权重占比就需要较大),这时候你的R矩阵就定义的大一些,比如定义为[[1,0],[0,1],这时候Q矩阵就可以定义的小一些,比如定义为[[0.11,0],[0,0.11]]。

想要获得较好的滤波预测效果,得不断的调试这两个矩阵的大小。

下面是详细的代码部分:

import numpy as np
import matplotlib.pyplot as plt

##下面是通过高斯正态分布产生误差
def gaussian_distribution_generator(var):
    return np.random.normal(loc=0.0, scale=var, size=None)  ##np.random.normal是正态分布

# 参数loc(float):正态分布的均值,对应着这个分布的中心。loc=0说明这一个以Y轴为对称轴的正态分布,
# 参数scale(float):正态分布的标准差,对应分布的宽度,scale越大,正态分布的曲线越矮胖,scale越小,曲线越高瘦。
# 参数size(int 或者整数元组):输出的值赋在shape里,默认为None。

# 状态转移矩阵,上一时刻的状态转移到当前时刻
A = np.array([[1, 1],
              [0, 1]])

# 过程噪声协方差矩阵Q,p(w)~N(0,Q),噪声来自真实世界中的不确定性      N(0,Q) 表示期望是0,协方差矩阵是Q
Q = np.array([[0.1, 0],              # Q其实就是过程的协方差
              [0, 0.1]])             # 过程的协方差越小,说明预估的越准确

# 观测噪声协方差矩阵R,p(v)~N(0,R)     也是测量的协方差矩阵
R = np.array([[1, 0],                # R其实就是测量的协方差矩阵
              [0, 1]])               #测量的协方差越小,说明测量的结果更准确

# 状态观测矩阵
H = np.array([[1, 0],
              [0, 1]])

# 控制输入矩阵B
B = None

# 初始位置与速度
X0 = np.array([[0],                  # 二维的,定义初始的位置和速度,初始的位置为0 ,速度为1
               [1]])

# 状态估计协方差矩阵P初始化(其实就是初始化最优解的协方差矩阵)
P = np.array([[1, 0],
              [0, 1]])

if __name__ == "__main__":
    # ---------------------------初始化-------------------------
    X_true = np.array(X0)           # 真实状态初始化
    X_posterior = np.array(X0)      # X_posterior表示上一时刻的最优估计值
    P_posterior = np.array(P)       # P_posterior是继续更新最优解的协方差矩阵

    speed_true = []                 # 实际的速度值
    position_true = []              # 实际的位置值

    speed_measure = []             # 测量到的速度值
    position_measure = []          # 测量到的位置值

    speed_prior_est = []           # 速度的先验误差 估计值
    position_prior_est = []        # 位置的先验误差 估计值

    speed_posterior_est = []       # 根据估计值及当前时刻的观测值融合到一体得到的最优估计速度值存入到列表中
    position_posterior_est = []    # 根据估计值及当前时刻的观测值融合到一体得到的最优估计值位置值存入到列表中

    for i in range(30):
        # -----------------------生成真实值----------------------
        # 生成过程噪声
        # 通过Q矩阵产生误差
        w = np.array(
            [[gaussian_distribution_generator(Q[0, 0])],         # gaussian_distribution_generator(Q[0, 0])的值为0.01103097596,(这值是随机的,每次运行都不一样)
             [gaussian_distribution_generator(Q[1, 1])]])        # gaussian_distribution_generator(Q[1, 1])的值为-0.1242726240,(这值是随机的,每次运行都不一样)
        X_true = np.dot(A, X_true) + w                           # 得到当前时刻实际的速度值和位置值,其中A是状态转移矩阵上一时刻的状态转移到当前时刻
        speed_true.append(X_true[1, 0])                          # 将第二行第一列位置的数值,1.01103098添加到列表speed_true里面
        position_true.append(X_true[0, 0])                       # 将第一行第一列位置的数值,1.01103098添加到列表position_true里面

        # -----------------------生成观测值----------------------
        # 生成观测噪声
        ##通过R矩阵产生误差
        v = np.array(
            [[gaussian_distribution_generator(R[0, 0])],         # gaussian_distribution_generator(R[0, 0])的值为-0.62251186549,(这值是随机的,每次运行都不一样)
             [gaussian_distribution_generator(R[1, 1])]])        # gaussian_distribution_generator(R[1, 1])的值为2.52779100481,(这值是随机的,每次运行都不一样)
        Z_measure = np.dot(H, X_true) + v                        # 生成观测值,H为单位阵E    # A是状态观测矩阵    # Z_measure表示测量得到的值
        position_measure.append(Z_measure[0, 0])
        speed_measure.append(Z_measure[1, 0])

        ################################################################下面开始进行预测和更新,来回不断的迭代#######################################################################

        # ----------------------进行先验估计---------------------
        X_prior = np.dot(A,X_posterior)                          # X_prior表示根据上一时刻的最优估计值得到当前的估计值  X_posterior表示上一时刻的最优估计值
        position_prior_est.append(X_prior[0, 0])                 # 将根据上一时刻计算得到的最优估计位置值添加到列表position_prior_est中
        speed_prior_est.append(X_prior[1, 0])                    # 将根据上一时刻计算得到的最优估计速度值添加到列表speed_prior_est.append中

        # 计算状态估计协方差矩阵P
        P_prior_1 = np.dot(A,P_posterior)                        # P_posterior是上一时刻最优估计的协方差矩阵   # P_prior_1就为公式中的(A.Pk-1)
        P_prior = np.dot(P_prior_1, A.T) + Q                     # P_prior是得出当前的先验估计协方差矩阵    #Q是过程协方差

        # ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posterior
        k1 = np.dot(P_prior, H.T)                                # P_prior是得出当前的先验估计协方差矩阵
        k2 = np.dot(np.dot(H, P_prior), H.T) + R                 # R是测量的协方差矩阵
        K = np.dot(k1, np.linalg.inv(k2))                        # np.linalg.inv():矩阵求逆   # K就是当前时刻的卡尔曼增益

        # ---------------------后验估计------------
        X_posterior_1 = Z_measure - np.dot(H, X_prior)           # X_prior表示根据上一时刻的最优估计值得到当前的估计值   # Z_measure表示测量得到的值
        X_posterior = X_prior + np.dot(K, X_posterior_1)         # K就是当前时刻的卡尔曼增益    # X_posterior是根据估计值及当前时刻的观测值融合到一体得到的最优估计值
        position_posterior_est.append(X_posterior[0, 0])         # 根据估计值及当前时刻的观测值融合到一体得到的最优估计位置值存入到列表中
        speed_posterior_est.append(X_posterior[1, 0])            # 根据估计值及当前时刻的观测值融合到一体得到的最优估计速度值存入到列表中

        # 更新状态估计协方差矩阵P     (其实就是继续更新最优解的协方差矩阵)
        P_posterior_1 = np.eye(2) - np.dot(K, H)                 # np.eye(2)返回一个二维数组,对角线上为1,其他地方为0
        P_posterior = np.dot(P_posterior_1, P_prior)             # P_posterior是继续更新最优解的协方差矩阵  ##P_prior是得出当前的先验估计协方差矩阵

    # 可视化显示
    if True:
        plt.rcParams['font.sans-serif'] = ['SimHei']  # 坐标图像中显示中文
        plt.rcParams['axes.unicode_minus'] = False

        fig, axs = plt.subplots(1, 2)
        axs[0].plot(speed_true, "-", label="speed_true(实际值)", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_measure, "-", label="speed_measure(测量值)", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_prior_est, "-", label="speed_prior_est(估计值)", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est(融合测量值和估计值)",
                    linewidth=1)  # Plot some data on the axes.
        axs[0].set_title("speed")
        axs[0].set_xlabel('k')  # Add an x-label to the axes.
        axs[0].legend()  # Add a legend.

        axs[1].plot(position_true, "-", label="position_true(实际值)", linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_measure, "-", label="position_measure(测量值)", linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_prior_est, "-", label="position_prior_est(估计值)",
                    linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_posterior_est, "-", label="position_posterior_est(融合测量值和估计值)",
                    linewidth=1)  # Plot some data on the axes.
        axs[1].set_title("position")
        axs[1].set_xlabel('k')  # Add an x-label to the axes.
        axs[1].legend()  # Add a legend.

        plt.show()

最后的运行结果见下:
快速上手的Python版二维卡尔曼滤波解析
以上就是二维卡尔曼滤波的相关解析和代码,仔细看,仔细品,你看得懂的。
快速上手的Python版二维卡尔曼滤波解析文章来源地址https://www.toymoban.com/news/detail-415616.html

到了这里,关于快速上手的Python版二维卡尔曼滤波解析的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 基于卡尔曼滤波的信号处理以及图像追踪Python实现

    这篇文章将由浅入深,从最简单的信号处理开始,到计算机视觉图像跟踪的应用。该文章使用Python语言,在进行视觉处理时,并未使用opencv自带的api,而是从矩阵运算进行逐步处理,更易于理解。 网上现在能看到的原理解释都很详细,这里就不多介绍。本人理解卡尔曼滤波的

    2024年02月05日
    浏览(53)
  • 【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 本文包括

    2024年02月08日
    浏览(43)
  • 卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)相应推导

    从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献。为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这些方法在姿态解算、轨迹规划等方面有着很多用途。卡尔

    2024年02月03日
    浏览(62)
  • 基于卡尔曼滤波的视频跟踪,基于卡尔曼滤波的运动小球跟踪

    完整代码和数据下载链接:基于卡尔曼滤波的视频跟踪,基于卡尔曼滤波的运动小球跟踪(代码完整,数据齐全)资源-CSDN文库 https://download.csdn.net/download/abc991835105/88738577 卡尔曼滤波原理 RBF的定义 RBF理论 易错及常见问题 RBF应用实例,基于rbf的空调功率预测 代码 结果分析

    2024年02月02日
    浏览(46)
  • 卡尔曼滤波理论小释之卡尔曼增益

    卡尔曼增益是卡尔曼滤波理论中的一个核心概念。一般教材里面是这么给出它的公式的: 图1  卡尔曼增益 直觉上容易理解,所谓的增益是指每次融合数据后不确定性的变化程度。如果融合了新的数据后不确定性降低了,那么这个增益就是正面的,有助于提高预测的准确度。

    2024年02月05日
    浏览(83)
  • 卡尔曼滤波器-概述及用递归思想解读卡尔曼滤波器 | 卡尔曼滤波器应用举例(附Matlab程序)| 数学基础-数据融合、协方差矩阵、状态空间方程

      卡尔曼滤波器是最优化的(Optimal)、递归的(Recursive)、数字处理的(Data Processing)算法(Algorithm)。卡尔曼滤波器更像是观测器,而不是一般意义上的滤波器,应用广泛,尤其是在导航中,它的广泛应用是因为生活中存在大量的不确定性。   当描述一个系统的不确

    2024年02月06日
    浏览(51)
  • 卡尔曼滤波系列_实例(二)均加速运动的卡尔曼滤波

    此系列(一)对卡尔曼滤波的原理进行了简单的阐述,总结了卡尔曼滤波的两大过程:预测和更新。接下来举例对卡尔曼滤波的使用进行介绍,加深对卡尔曼滤波的理解。 1.场景介绍 如上图所示,可知小车的初始速度为0,初始位置也为0,小车向前的加速度为1,小车感知自身

    2024年02月15日
    浏览(36)
  • 卡尔曼滤波学习笔记

    从直观上来看,卡尔曼滤波是把两个存在误差的结果 融合 在一起,得到一个从数学上可以得到证明的 最优估计值 。 而这两个存在误差的结果,一个是从理论上推导出来的,称之为 先验估计值 ;一个是用传感器测量出来的,称之为 测量值 。它们之所以存在误差,是因为前

    2024年02月11日
    浏览(42)
  • 卡尔曼滤波介绍

        卡尔曼滤波无论是在单目标还是多目标领域都是很常用的一种算法,将卡尔曼滤波看作一种运动模型,用来对目标的位置进行预测,并且利用预测结果对跟踪的目标进行修正,属于自动控制理论中的一种方法。     在对视频中的目标进行跟踪时,当 目标运动速度较慢 时

    2024年02月14日
    浏览(38)
  • 【算法系列】卡尔曼滤波算法

    ·【算法系列】卡尔曼滤波算法 ·【算法系列】非线性最小二乘求解-直接求解法 ·【算法系列】非线性最小二乘求解-梯度下降法 ·【算法系列】非线性最小二乘-高斯牛顿法  ·【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法  文章目录 系列文章 文章目录 前言 一、

    2024年02月12日
    浏览(38)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包