Python Kalman filter for mouse position isn't working as expected

Question

I started using python with Jupyter notebook, so for learning purpose I implemented Kalman filter following this YouTube tutorial. I first use my Kalman filter with static data, I guess it was working so I tried to extend that code to apply filter on mouse coordinate but it is behaving really weired. Can anyone help me to fix?

I expect to have this kind of result but I am getting something like the screenshot.

here is the code -

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('', 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()

red one is Kalman and blue is actual

Edit : updated with code suggested by@Lho

user_11336002 · Answer

I think what you are missing is a process model. A strategy like this one works if you are measuring a static parameter, but if the state you are tracking is itself changing, as it is here, you need a process model that takes those changes into consideration. You will do a process model estimation step before each update step.

The process model in this case will involve some basic Newtonian physics. I would suggest expanding the state you are tracking (x and y) to include velocities (vx, vy) and accelerations (ax, ay). So instead of [x y]^T, your state vector would be X = [x y vx vy ax ay]^T. (You can derive the velocities based on the amount of movement per time step, and you can either try to derive real accelerations from movement over several time steps, or it might be good enough to just assume a reasonable acceleration, since the true acceleration in this case is likely going to be erratic and can't easily be modeled.) You also need a process matrix F that transforms X into the measurement vector Z (i.e., [x y]^T, the fuzzy measurements you are taking). In this case, F will be a 2x6 matrix.

user_5844350 · Answer

I didn't run your code, but your variables Err_est_x and Err_est_y are only getting smaller? You must increase these variables after each call of updateCall like this:

# 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

Explanation: If your estimated error is low, the information of a new noisy measurment is also low [1]. This is also visible in your visualisation: Initially, the red kalman estimate is 'very fast' because your estimated error is high. But when your estimated error is getting smaller, your red kalman estimate is 'slowing done'.

Please note that I didn't run your code, so the chosen values (1.1 and 0.1) may be to high or too small. Increase/reduce both numbers to increase/reduce the 'speed' of your red kalman estimate.

[1] For example, if your estimated error is 0, the information gain of a new measurment is also 0.

Python Kalman filter for mouse position isn't working as expected

2 Answers