卡尔曼滤波之二:Python实现

卡尔曼滤波之二:Python实现

了解了卡尔曼滤波之一:基本概念,可以结合代码来理解下卡尔曼滤波的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=xt−1+vt−1τ−21gτ2

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

其中, τ \tau τ 为 t − 1 t-1 t−1 与 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−=F⋅x^t−1+B⋅ut−1 ①

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 \[ht−vt−\]=\[10τ1\]\[ht−1vt−1\]+\[−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−=F⋅Pt−1⋅FT+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=H⋅Pt−⋅HT+RPt−⋅HT ③ \\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(Zt−H⋅x\^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=(1−Kt⋅H)Pt− ⑤ ## 3.代码实现 ## 3.1 输入值 ```python 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包实现 ```python 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} B⋅ut−1部分。 ### 3.3 不使用Python包实现 ```python current_state_covariance=None current_state_mean=None time=0 estimated_signal = [] for measurement in sim: # predict if time==0: # initialize predicted_state_means=initial_state predicted_state_covariances=initial_current_state_covariance else: predicted_state_means = F @ current_state_mean+ B*U predicted_state_covariances = F @current_state_covariance @F.T + Q # update kalman_gain = predicted_state_covariances @ H.T @ np.linalg.pinv(H@[email protected] + 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_covariances estimated_signal.append(current_state_mean[0,0]) time + =1 ``` ### 3.4 效果可视化 ```python import matplotlib matplotlib.use("TkAgg") import matplotlib.pyplot as plt plt.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) ``` ![在这里插入图片描述](https://file.jishuzhan.net/article/1721899744306925569/2772cfb9c0826f07ea14cba187f81db1.webp) 两种方法曲线重合在一起,说明Python实现没有问题。 注意:这里的两种实现都默认`t=0`时只赋初值,不进行预测。 [信号去噪之卡尔曼滤波](https://mp.weixin.qq.com/s/QKV9hPNCYDVKC1oE6jg6pA)代码实现中,`t=0`时也进行了预测。 长期来看,效果差不多, 从图上可以看出,滤波信号与有噪声信号相比,非常平滑,同时也有很好的跟踪效果。 ## 参考文献 \[1\] [信号去噪之卡尔曼滤波](https://mp.weixin.qq.com/s/QKV9hPNCYDVKC1oE6jg6pA) \[2\] [卡尔曼滤波:再也不用瑟瑟发抖了](https://mp.weixin.qq.com/s?__biz=MzU5NTg2MzIxMw==&mid=2247500991&idx=1&sn=0ccc29c905d4f97326d45494977c312d&chksm=fe69f77dc91e7e6b90dfba22d9cf531ef5843f3f113e1296ed89d4d48e8209e5809aad167dae&scene=132&exptype=timeline_recommend_article_extendread_extendread_for_notrec#wechat_redirect) \[3\] \[4\] \[5\] \[6\] [Kalman Filter and Maximum Likelihood Estimation of Linearized DSGE Models](http://mx.nthu.edu.tw/~tkho/DSGE/lecture4.pdf) \[7\] [卡尔曼滤波之一:基本概念](https://blog.csdn.net/WANGWUSHAN/article/details/124119597)

相关推荐
Evand J3 天前
【MATLAB例程】三维环境下,动态轨迹的AOA定位与UKF滤波,模拟IMU/AOA的数据融合(AOA的测角基站数量可自适应,目标运动轨迹可自行修改)
开发语言·算法·matlab·卡尔曼滤波
软件算法开发18 天前
基于卡尔曼滤波的雷达光电多目标航迹融合算法matlab仿真
算法·matlab·卡尔曼滤波·雷达光电·多目标航迹融合
大福是小强4 个月前
042_Unscented Kalman Filter in Matlab无迹卡尔曼滤波
matlab·卡尔曼滤波·状态估计·ukf·无迹卡尔曼滤波·系统辨识
Evand J4 个月前
自适应卡尔曼滤波(包括EKF、UKF、CKF等)的创新思路——该调什么、不该调什么
开发语言·笔记·matlab·卡尔曼滤波·自适应滤波
肥猪猪爸4 个月前
使用卡尔曼滤波器估计pybullet中的机器人位置
数据结构·人工智能·python·算法·机器人·卡尔曼滤波·pybullet
chenxiemin5 个月前
卡尔曼滤波:从理论到应用的简介
python·算法·filter·卡尔曼滤波·kalman
阿白机器人5 个月前
卡尔曼滤波器-Kalmen Filter-1
卡尔曼滤波·处理算法
xiaoyaolangwj8 个月前
高翔【自动驾驶与机器人中的SLAM技术】学习笔记(五)卡尔曼滤波器一:认知卡尔曼滤波器;协方差矩阵与方差;
算法·机器人·自动驾驶·卡尔曼滤波
西西弗Sisyphus10 个月前
DeepSORT(目标跟踪算法)卡尔曼滤波中的贝叶斯定理
人工智能·算法·目标跟踪·卡尔曼滤波·多目标跟踪·deepdort·卡尔曼增益
前端讲堂-鸿蒙课堂10 个月前
卡尔曼滤波(Kalman Filtering)详细解读
卡尔曼滤波