用于鼠标位置的Python卡尔曼滤波器未按预期工作

Question

我开始在Jupyter笔记本上使用python，因此出于学习目的，我在下面的YouTube tutorial中实现了卡尔曼滤波器。我首先对静态数据使用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, 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()
 

红色是Kalman，蓝色是实际值

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

Answer 1

我认为您缺少的是一个过程模型。如果您正在测量一个静态参数，那么像这样的策略是有效的，但是如果您正在跟踪的状态本身正在变化，就像这里一样，那么您需要一个考虑这些变化的流程模型。您将在每个更新步骤之前执行流程模型估计步骤

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