鼠标位置的Python Kalman过滤器无法正常工作

问题描述

我开始在Jupyter笔记本中使用python，因此出于学习目的，我在此YouTube tutorial之后实现了Kalman过滤器。我首先将Kalman过滤器与静态数据一起使用，我猜想它在起作用，因此我尝试扩展该代码以将过滤器应用于鼠标坐标，但是它确实很奇怪。谁能帮我修复？

我希望有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,Est,Err_est,Err_msr):
    kg = getKG(Err_est,Err_msr)
    Est = getEst(Est,val)
    Err_est = getE_est(kg,Err_est)
    return kg,Err_est



kg_x,Est_x,Err_est_x = updateKal(true_x,kg_x,Err_est_x,Err_msr_x)
kg_y,Est_y,Err_est_y = updateKal(true_y,kg_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,true_y
    global kg_x,Err_est_x
    global kg_y,Err_est_y
    global firstval

    x,y = e.x,e.y

    kg_x,Err_est_x = updateKal(x,0.5)
    kg_y,Err_est_y = updateKal(y,0.5)

    cv.create_line((lastx,lasty,x,y),fill='blue',width=1)
    cv.create_line((true_x,true_y,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()
 

红色是Kalman，蓝色是实际

编辑：已使用@Lho建议的代码更新

解决方法

我认为您缺少的是流程模型。如果要测量静态参数，则可以采用这种策略，但是，如果要跟踪的状态本身正在发生变化（如此处所示），则需要一个将这些变化考虑在内的过程模型。您将在每个更新步骤之前执行流程模型估计步骤。

在这种情况下，过程模型将涉及一些基本的牛顿物理学。我建议将要跟踪的状态（x和y）扩展为包括速度（vx，vy）和加速度（ax，ay）。因此，您的状态向量将不是[x y] ^T ，而是 X = [x y vx vy ax ay] ^T 。 （您可以根据每个时间步长的运动量得出速度，也可以尝试从多个时间步长的运动中得出实际加速度，或者也可以假设一个合理的加速度就足够了，因为您可能还需要一个过程矩阵 F ，该矩阵将 X 转换为测量向量 Z （即[xy] ^T ，即您要进行的模糊测量）。在这种情况下， F 将是2x6的矩阵。

,

我没有运行您的代码，但是您的变量Err_est_x和Err_est_y越来越小？您必须在每次调用updateCall之后增加这些变量，如下所示：

# in method paint()
#
kg_x,Est_x,Err_est_x = updateKal(x,kg_x,Err_est_x,0.5)
kg_y,Est_y,Err_est_y = updateKal(y,kg_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]。 这在您的可视化效果中也是可见的：最初，红色卡尔曼估计值“非常快”，因为您估计的误差很高。但是，当您估计的误差变得越来越小时，您的红色kalman估计就“变慢了”。

请注意，我没有运行您的代码，因此选择的值（1.1和0.1）可能太大或太小。增加/减少这两个数字以增加/减少红色卡尔曼估计的“速度”。

[1]例如，如果您的估计误差为0，则新测量的信息增益也为0。