用于鼠标位置的 Python Kalman 滤波器未按预期工作
Posted
技术标签:
【中文标题】用于鼠标位置的 Python Kalman 滤波器未按预期工作【英文标题】:Python Kalman filter for mouse position isn't working as expected 【发布时间】:2020-10-01 16:47:28 【问题描述】:我开始在 Jupyter notebook 中使用 python,所以为了学习目的,我在YouTube tutorial 之后实现了卡尔曼滤波器。我首先将我的卡尔曼滤波器与静态数据一起使用,我猜它正在工作,所以我尝试扩展该代码以在鼠标坐标上应用滤波器,但它的行为真的很奇怪。谁能帮我解决?
我希望有this kind of result,但我得到了类似屏幕截图的东西。
这里是代码 -
import numpy as np
import pandas as pd
from tkinter import *
### True Value: What should be the actual value if there was no error in measurements
### Estimates : It is the predicted value.
#### Error in Estimate: Error in the predicted value
### Measurements : It is the actual value measured with sensor
#### Error in Measurements: Error in the measured value
#### initializing variables
true_x,true_y = 0,0 # True Mouse location
Est_x,Est_y = 0,0 # Initial Estimate
Err_est_x,Err_est_y = 5,5 # Error in Initial Estimate
M_x,M_y = 0,0 # Initial Measurements
Err_msr_x,Err_msr_y = 5,5 # Error in Initial Measurements
kg_x,kg_y = 0,0 # Kalman Gain
def getKG(Err_est, Err_msr):
return Err_est/(Err_est+Err_msr)
def getEst(EST_prev, kg, Msr):
return EST_prev+kg*(Msr-EST_prev)
def getE_est(kg, Err_est):
return (1-kg)*Err_est
def updateKal(val, kg, Est, Err_est, Err_msr):
kg = getKG(Err_est, Err_msr)
Est = getEst(Est, kg, val)
Err_est = getE_est(kg, Err_est)
return kg, Est, Err_est
kg_x,Est_x,Err_est_x = updateKal(true_x,kg_x,Est_x,Err_est_x,Err_msr_x)
kg_y,Est_y,Err_est_y = updateKal(true_y,kg_y,Est_y,Err_est_y,Err_msr_y)
def activate_paint(e):
global lastx, lasty
global true_x, true_y
cv.bind('<B1-Motion>', paint)
lastx, lasty = e.x, e.y
true_x, true_y = e.x, e.y
Est_x, Est_y = e.x, e.y
def paint(e):
global lastx, lasty
global true_x, true_y
global kg_x, Est_x, Err_est_x
global kg_y, Est_y, Err_est_y
global firstval
x, y = e.x, e.y
kg_x, Est_x, Err_est_x = updateKal(x, kg_x, Est_x, Err_est_x, 0.5)
kg_y, Est_y, Err_est_y = updateKal(y, kg_y, Est_y, Err_est_y, 0.5)
cv.create_line((lastx, lasty, x, y), fill='blue',width=1)
cv.create_line((true_x,true_y, Est_x, Est_y), fill='green',width=2)
lastx, lasty = x, y
true_x, true_y = Est_x, Est_y
root = Tk()
lastx, lasty = None, None
cv = Canvas(root, width=640, height=480, bg='white')
cv.bind('<1>', activate_paint)
cv.pack(expand=YES, fill=BOTH)
root.mainloop()
红色的是卡尔曼,蓝色的是实际
编辑:使用@Lho 建议的代码更新
【问题讨论】:
奇怪是什么意思?你想达到什么目的? @Atlas435 检查我添加的屏幕截图。从技术上讲,它应该与这些视频中的鼠标实际位置一样接近。 youtube.com/watch?v=Isv5MEeaKjU 我不知道 PIL 的用途是什么。我会构建自己的代码来做到这一点。我想我在这里帮不上忙,但你可以看看这个,也许可以解码那个。 github.com/yanfeit/Kalman-Filter @Atlas435 忘记仅用于绘制画布的 PIL。这里的关键是卡尔曼增益 (KG) 和估计值 (Est) 计算不正确。我按照本教程进行操作 - youtube.com/watch?v=SIQJaqYVtuE&ab_channel=MichelvanBiezen 【参考方案1】:我没有运行您的代码,但您的变量 Err_est_x
和 Err_est_y
只会越来越小?您必须在每次调用 updateCall
后增加这些变量,如下所示:
# in method paint()
#
kg_x, Est_x, Err_est_x = updateKal(x, kg_x, Est_x, Err_est_x, 0.5)
kg_y, Est_y, Err_est_y = updateKal(y, kg_y, Est_y, Err_est_y, 0.5)
Err_est_x = 1.1 * Err_est_x + 0.1 # new code
Err_est_y = 1.1 * Err_est_y + 0.1 # new code
解释:如果您的估计误差较低,则新的噪声测量的信息也较低 [1]。 这在您的可视化中也很明显:最初,红色卡尔曼估计“非常快”,因为您的估计误差很高。但是,当您的估计误差越来越小时,您的红卡尔曼估计正在“减慢完成”。
请注意,我没有运行您的代码,因此选择的值(1.1 和 0.1)可能过高或过小。增加/减少这两个数字以增加/减少红卡尔曼估计的“速度”。
[1] 例如,如果您的估计误差为 0,则新测量的信息增益也为 0。
【讨论】:
谢谢伙计。我知道我的值越来越小,这导致了 gif 中显示的问题。尽管您的解决方案完美运行。我从其他教程中了解到的是 - 我必须使用您的解决方案看起来像的运动方程(初始位置、最终位置、加速度)。【参考方案2】:我认为您缺少的是流程模型。如果您正在测量静态参数,则类似这样的策略有效,但如果您正在跟踪的状态本身正在发生变化,就像这里一样,您需要一个将这些变化考虑在内的过程模型。您将在每个更新步骤之前执行流程模型估计步骤。
本例中的过程模型将涉及一些基本的牛顿物理学。我建议扩展您正在跟踪的状态(x 和 y)以包括速度(vx,vy)和加速度(ax,ay)。因此,您的状态向量将是 X = [x y vx vy ax ay]T,而不是 [x y]T。 (您可以根据每个时间步的移动量推导出速度,并且您可以尝试从多个时间步的移动中推导出实际加速度,或者假设一个合理的加速度就足够了,因为真正的加速度在这种情况可能会不稳定并且不容易建模。)您还需要一个过程矩阵 F 将 X 转换为测量向量 Z(即 [x y]T,您正在进行的模糊测量)。在这种情况下,F 将是一个 2x6 矩阵。
【讨论】:
以上是关于用于鼠标位置的 Python Kalman 滤波器未按预期工作的主要内容,如果未能解决你的问题,请参考以下文章
滤波跟踪基于Kalman滤波的视频目标跟踪matlab 源码
. 细说Kalman滤波:The Kalman Filter
自动驾驶 9-3: 走向非线性 - 扩展卡尔曼滤波器 Going Nonlinear - The Extended Kalman Filter
自动驾驶 9-3: 走向非线性 - 扩展卡尔曼滤波器 Going Nonlinear - The Extended Kalman Filter