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

Question

这是代码-

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

红色的是卡尔曼，蓝色的是实际的

Answer 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 codeErr_est_y = 1.1 * Err_est_y + 0.1 # new code解释：如果您的估计误差较低，则新的噪声测量的信息也较低 [1]。这在您的可视化中也可见：最初，红色卡尔曼估计“非常快”，因为您的估计误差很高。但是，当您的估计误差越来越小时，您的红色卡尔曼估计就会“减慢完成”。请注意，我没有运行您的代码，因此所选值（1.1 和 0.1）可能过高或过小。增加/减少这两个数字以增加/减少红色卡尔曼估计的“速度”。[1] 例如，如果您的估计误差为 0，则新测量的信息增益也为 0。

Answer 2

我认为您缺少的是流程模型。如果您正在测量静态参数，这样的策略是有效的，但如果您正在跟踪的状态本身正在发生变化，就像这里一样，您需要一个过程模型来考虑这些变化。您将在每个更新步骤之前执行流程模型估计步骤。本例中的过程模型将涉及一些基本的牛顿物理学。我建议扩展您正在跟踪的状态（x 和 y）以包括速度（vx，vy）和加速度（ax，ay）。因此，您的状态向量将是X&nbsp;= [xy vx vy ax ay]&nbsp;T ，而不是 [xy]&nbsp;T。（您可以根据每个时间步长的运动量得出速度，并且可以尝试从多个时间步长的运动中得出实际加速度，或者假设一个合理的加速度可能就足够了，因为真实加速度这种情况可能会不稳定并且不容易建模。）您还需要一个过程矩阵F来将X转换为测量向量Z（即， [xy]&nbsp;T，您正在进行的模糊测量）。在这种情况下，F将是一个 2x6 矩阵。