卡尔曼滤波之二:Python实现

卡尔曼滤波之二:Python实现

  • 1.背景描述
  • 2.构建卡尔曼滤波公式
    • 2.1 预测
    • 2.2 更新
  • 3.代码实现
  • 3.1 输入值
    • 3.2 pykalman包实现
    • 3.3 不使用Python包实现
    • 3.4 效果可视化
  • 参考文献

了解了卡尔曼滤波之一:基本概念,可以结合代码来理解下卡尔曼滤波的2个预测+3个更新环节。

1.背景描述

设有一个球在30m的起始高度,以10m/s的速度竖直上抛,用传感器来跟踪球的高度。

对应于卡尔曼滤波,此系统的状态包括位置 h h h及速度 v v v

球的高度满足式子:

x t = x t − 1 + v t − 1 τ − 1 2 g τ 2 \qquad x_t = x_{t-1} + v_{t-1}\tau - \frac{1}{2} g \tau^2 xt=xt1+vt1τ21gτ2

速度满足:
v t = v t − 1 − g τ \qquad v_t = v_{t-1} - g \tau vt=vt1

其中, τ \tau τ t − 1 t-1 t1 t t t之间的时间(s), g g g 为重力加速度。

传感器检测高度,存在的位置协方差约为3m。

2.构建卡尔曼滤波公式

2.1 预测

  • 状态值预测

x ^ t − = F ⋅ x ^ t − 1 + B ⋅ u t − 1 ① \qquad\qquad\quad \hat x_t^-=F\cdot\hat x_{t-1} + B\cdot u_{t-1}\qquad\qquad \qquad\qquad \qquad\qquad \quad\ \ \ ① x^t=Fx^t1+But1   

[ h t − v t − ] = [ 1 τ 0 1 ] [ h t − 1 v t − 1 ] + [ − 1 2 τ 2 − τ ] ⋅ g \qquad\qquad\begin{bmatrix} h_t^-\\v_t^-\end{bmatrix} =\begin{bmatrix} 1& \tau \\0 & 1 \end{bmatrix}\begin{bmatrix} h_{t-1}\\v_{t-1}\end{bmatrix}+\begin{bmatrix} - \frac{1}{2} \tau^2\\- \tau\end{bmatrix}\cdot g [htvt]=[10τ1][ht1vt1]+[21τ2τ]g

  • 状态协方差预测

P t − = F ⋅ P t − 1 ⋅ F T + Q ② \qquad\qquad \quad P_{t}^{-}=F\cdot P_{t-1}\cdot F^{T}+Q\qquad\qquad \qquad\qquad \qquad\qquad \qquad ② Pt=FPt1FT+Q

\qquad 状态协方差预测值的初始值 P 0 − P_{0}^{-} P0 [ 1 0 0 1 ] \begin{bmatrix} 1& 0 \\0 & 1 \end{bmatrix} [1001],过程噪声的协方差 Q Q Q [ 0 0 0 0 ] \begin{bmatrix} 0& 0 \\0 & 0 \end{bmatrix} [0000]

2.2 更新

  • 更新卡尔曼增益 K t K_{t} Kt

K t = P t − ⋅ H T H ⋅ P t − ⋅ H T + R ③ \qquad\qquad K_{t}=\cfrac {P_{t}^{-} \cdot H^{T}}{H\cdot P_{t}^{-} \cdot H^{T}+R}\qquad\qquad \qquad\qquad \qquad\qquad \qquad\ ③ Kt=HPtHT+RPtHT 

\qquad 观测矩阵 H H H 这里为 [ 1 0 ] \begin{bmatrix} 1& 0 \end{bmatrix} [10] R R R 为 3.

  • 融合状态估计值 x ^ t − \hat x_{t}^{-} x^t以及观测量 Z t Z_t Zt,更新状态值 x ^ t \hat x_{t} x^t

x ^ t = x ^ t − + K t ( Z t − H ⋅ x ^ t − ) ④ \qquad\qquad \hat x_{t}=\hat x_{t}^{-}+K_t(Z_t-H\cdot \hat x_{t}^{-})\qquad\qquad \qquad\qquad\qquad \qquad\ ④ x^t=x^t+Kt(ZtHx^t) 

  • 更新状态协方差​ P t P_{t} Pt

P t = ( 1 − K t ⋅ H ) P t − ⑤ \qquad\qquad P_{t}=(1-K_t\cdot H) P_{t}^{-}\qquad\qquad \qquad\qquad\qquad \qquad \qquad\ \ \ \ ⑤ Pt=(1KtH)Pt    

3.代码实现

3.1 输入值

import numpy as np
times = 40
tau = 0.1
actual = -4.9*tau**2*np.arange(times)**2
# Simulate the noisy camera data
sim = actual + 3*np.random.randn(times)# kalman filter parameters
initial_state = np.array([[30],[10]])  
initial_current_state_covariance =np.eye(2) 
Q = np.zeros((2,2)) # process_noise_covariance
R = 3 # observation_covariance
F=np.array([[1,tau],[0,1]])
B=np.array([[-0.5*tau**2],[-tau]])
U=g=9.8
H=np.array([[1,0]])

3.2 pykalman包实现

from pykalman import KalmanFilter
# Set up the filter
kf = KalmanFilter(n_dim_obs=1, n_dim_state=2,initial_state_mean=initial_state.reshape(-1) ,initial_state_covariance=initial_current_state_covariance ,transition_matrices=F,observation_matrices=H,observation_covariance=R,transition_covariance=Q,transition_offsets=[-4.9*tau**2, -9.8*tau])state_means, state_covs = kf.filter(sim)

注意,pykalman包中使用transition_offsets来替代 B ⋅ u t − 1 B\cdot u_{t-1} But1部分。

3.3 不使用Python包实现

current_state_covariance=None
current_state_mean=None
time=0
estimated_signal = []
for measurement in sim:# predictif time==0:# initialize predicted_state_means=initial_statepredicted_state_covariances=initial_current_state_covarianceelse:predicted_state_means = F @ current_state_mean+ B*Upredicted_state_covariances = F @current_state_covariance @F.T + Q# updatekalman_gain = predicted_state_covariances @ H.T @ np.linalg.pinv(H@predicted_state_covariances@H.T + R)current_state_mean = predicted_state_means  + kalman_gain * (measurement - H @ predicted_state_means)current_state_covariance = predicted_state_covariances - kalman_gain @ H @ predicted_state_covariancesestimated_signal.append(current_state_mean[0,0])time + =1

3.4 效果可视化

import matplotlib
matplotlib.use("TkAgg")
import matplotlib.pyplot as pltplt.figure(figsize=(12, 6))
plt.plot(range(times), sim, label="Noisy Signal")
plt.plot(range(times), state_means[:,0], label="Kalman Signal1")plt.plot(range(times), estimated_signal, label="Estimated Signal (Kalman Filter)")
plt.legend()
plt.title("Signal Denoising with Kalman Filter")
plt.show(block=True)

在这里插入图片描述
两种方法曲线重合在一起,说明Python实现没有问题。

注意:这里的两种实现都默认t=0时只赋初值,不进行预测。

信号去噪之卡尔曼滤波代码实现中,t=0时也进行了预测。

长期来看,效果差不多,

从图上可以看出,滤波信号与有噪声信号相比,非常平滑,同时也有很好的跟踪效果。

参考文献

[1] 信号去噪之卡尔曼滤波
[2] 卡尔曼滤波:再也不用瑟瑟发抖了
[3] https://github.com/quantopian/research_public/blob/master/notebooks/lectures/Kalman_Filters/notebook.ipynb
[4] https://github.com/pykalman/pykalman/tree/master
[5] https://pykalman.github.io/#choosing-parameters
[6] Kalman Filter and Maximum Likelihood Estimation of Linearized DSGE Models
[7] 卡尔曼滤波之一:基本概念

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.hqwc.cn/news/162775.html

如若内容造成侵权/违法违规/事实不符,请联系编程知识网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【数据库】数据库模式 Schema

数据库模式 Schema 1.MySQL2.PostgreSQL3.SQL Server4.Oracle5.SQLite 在数据库的术语中,模式(schema)是一个逻辑概念,用于组织数据库中的对象。模式中的对象通常包括 表、索引、数据类型、序列、视图、存储过程、主键、外键 等等…

Electron[3] 基础配置准备和Electron入门案例

1 背景 上一篇文章已经分享了,如何准备Electron的基础环境了。但是博客刚发才一天,就发现有人问问题了。经过实践发现,严格按照作者的博客教程走是不会有问题的,其中包括安装的环境版本等都要一致。因为昨天发的博客,…

【论文阅读】Equivariant Contrastive Learning for Sequential Recommendation

【论文阅读】Equivariant Contrastive Learning for Sequential Recommendation 文章目录 【论文阅读】Equivariant Contrastive Learning for Sequential Recommendation1. 来源2. 介绍3. 前置工作3.1 序列推荐的目标3.2 数据增强策略3.3 序列推荐的不变对比学习 4. 方法介绍4…

kafka问题汇总

报错1: 解决方式 1、停止docker服务   输入如下命令停止docker服务 systemctl stop docker 或者service docker stop1   停止成功的话,再输入docker ps 就会提示出下边的话: Cannot connect to the Docker daemon. Is the docker daem…

汽车标定技术(四)--问题分析:多周期测量时上位机显示异常

目录 1.问题现象 2.数据流分析 ​​​​3.代码分析 3.1 AllocDAQ 3.2 AllocOdt 3.3 AllocOdtEntry 4.根因分析及解决方法 4.1 根因分析 4.2 解决方案 1.问题现象 在手撸XCP代码时, DAQ的实现是一大头痛的事情。最初单周期实现还好一点,特别是…

聊一聊关于手机Charge IC的电流流向

关于手机Charge,小白在以前的文章很少讲,一是这部分东西太多,过于复杂。二是总感觉写起来欠缺点什么。但后来想一想,本是抱着互相学习来写文章的心理态度,还是决定尝试写一些。 关于今天要讲的关于手机Charge的内容&a…

Visual Studio Code 常用快捷键大全

Visual Studio Code 常用快捷键大全 快捷键是编码过程中经常使用,且能够极大提升效率的部分,这里给大家介绍一些VS Code中非常有用的快捷键。 打开和关闭侧边栏 Mac — Command B Windows — Ctrl B Ubuntu — Ctrl B 选择单词 Mac — Command D …

本地电脑监控软件

本地电脑监控软件是一种用于监控计算机使用行为的软件,它可以帮助企业管理者了解员工的工作状态和行为,保护企业的计算机资源。 本地电脑监控软件可以监控员工的计算机使用行为,包括屏幕监控、键盘记录、文件操作等。这些功能可以帮助企业管理…

FFmpeg直播能力更新计划与新版本发布

// 编者按:客户端作为直接面向用户大众的接口,随着技术的发展进化与时俱进,实现更好的服务是十分必要的。FFmpeg作为最受欢迎的视频和图像处理开源软件,被相关行业的大量用户青睐,而随着HEVC标准的发布到广泛使用&am…

ClickHouse开发系列

一、 ClickHouse详解、安装教程_clickhouse源码安装 二、ClickHouse 语法详解_clickhouse讲解 三、ClickHouse SQL 操作语句详解 四、ClickHouse 高级教程—官方原版 五、ClickHouse主键索引最佳实践 六、MySQL与ClickHouse集成 七、ClickHouse 集成MongoDB、Re…

制造业企业设备管理常见的三个问题及对应的解决方案

当今的市场如同茫茫大海,既充满机遇,也伴随着波动的风险。在现代制造业中,企业常常面临着各种挑战,这些挑战可能妨碍其发展和竞争力。但制造企业往往具备能够解决挑战的能力,借助软件工具的力量,可以更好地…

微信小程序-form表单-获取用户输入文本框的值

微信小程序-form表单-获取用户输入文本框的值 data: {userName: ,userPwd:""},//获取用户输入的用户名 userNameInput:function(e) {this.setData({userName: e.detail.value}) }, passWdInput:function(e) {this.setData({userPwd: e.detail.value}) }, //获取用户输…