# Kalman filter in one dimension to track a moving object

• Comp Sci
• Karagoz
In summary, the Kalman class in the code below is my attempt to solve the problem. However, the code doesn't work well. I have written these 5 equations in the Kalman-filter algorithm:State Extrapolation EquationCovarinace Extrapolation EquationKalman gain computation (computation of alpha, beta and gamma, computation of gamma is commented)State Update EquationCovariance Update EquationI have just written as it's described in different guides online like KalmanFilter.net, but when I run the code it doesn't work well. How suspect my equations are wrong. How do I correct those equations?
Karagoz
Homework Statement
Implement Kalman filter to track moving object that's moving back and forth between two points
Relevant Equations
Covariance Extrapolation Equations, Kalman gain computations, Covarinace Update Equations
Below is the whole code. I can't change the whole code, I can only change the "Kalman class".

The Kalman class in the code below is my attempt to solve the problem.

But the code doesn't work well.

I have written these 5 equations in the Kalman-filter algorithm:

State Extrapolation Equation

Covarinace Extrapolation Equation

Kalman gain computation (computation of alpha, beta and gamma, computation of gamma is commented)

State Update Equation

Covariance Update Equation

I have just written as it's described in different guides online like KalmanFilter.net

But when I run the code it doesn't work well.

How suspect my equations are wrong. How do I correct those equations?The Kalman class is between line 71 and 129.

The whole code:
'''

'''
import pygame as pg
from random import random,randint
import numpy as np
from numpy.linalg import norm

fps = 0.0

class Projectile():

def __init__(self, background, kalman=None):
self.background = background
self.rect = pg.Rect((800,700),(16,16))
self.px = self.rect.x
self.py = self.rect.y
self.dx = 0.0
self.kalm = kalman

def move(self,goal):

if self.kalm:
goal = self.kalm.calc_next(goal)
deltax = np.array(float(goal) - self.px)
#print(delta2)
mag_delta = norm(deltax)#* 500.0
np.divide(deltax,mag_delta,deltax)

self.dx += deltax
#if self.dx:
#self.dx /= norm(self.dx) * 50

self.px += self.dx /50.0
self.py += -0.5
try:
self.rect.x = int(self.px)
except:
pass
try:
self.rect.y = int(self.py)
except:
pass

class Target():

def __init__(self, background, width):
self.background = background
self.rect = pg.Rect(self.background.get_width()//2-width//2,
50, width, 32)
self.dx = 1 if random() > 0.5 else -1

def move(self):
self.rect.x += self.dx

if self.rect.x < 300 or self.rect.x > self.background.get_width()-300:
self.dx *= -1

def noisy_x_pos(self):
pos = self.rect.x
center = self.rect.width//2
noise = np.random.normal(0,1,1)[0]

return pos + center + noise*300.0
#
# Here is the Kalman filter I need to develop
#

class Kalman():
def __init__(self):
self._dt = 1  # time step
self._pos = 785 # initialized position
self._vel = 1 #initialized velocity
self._acc = 0.05 #initialized accelaration
self._pos_est_err = 1000000 #position estimation uncertainty
self._vel_est_err = 1000000 #velocity estimation uncertainty
self._acc_est_err = 2000 #acceleratione stimation uncertainty
self._pos_mea_err = 300 #measurement error
self._vel_mea_err = 100 #velocity measurement error
self._acc_mea_err = 4 #acceleration measurement error
self._alpha = 1 # converges to 0.32819526927045667
self._beta = 1 # converges to 0.18099751242241782
self._gamma = 0.01 # gamma

self._q = 30 # process uncerrainty for position
self._q_v = 4 # process uncertainty for velocity
self._q_a = 2 # process uncertainty for acceleration

self._last_pos = 785
self._measured_velocity = 0    def calc_next(self, zi):

self._measured_velocity = zi - self._last_pos
self._last_pos = zi

#State extrapolation
self._pos = self._pos + self._vel*self._dt + 0.5*self._acc*self._dt**2
self._vel = self._vel + self._acc*self._dt
self._acc = self._acc

#Covariance extrapolation
self._pos_est_err = self._pos_est_err + self._dt**2 * self._vel_est_err + self._q
self._vel_est_err = self._vel_est_err + self._q_v
self._acc_est_err = self._acc_est_err + self._q_a

#Alhpa-beta-gamma update
self._alpha = self._pos_est_err / (self._pos_est_err + self._pos_mea_err)
self._beta = self._vel_est_err / (self._vel_est_err + self._vel_mea_err)
#self._gamma = self._acc_est_err / (self._acc_est_err + self._acc_mea_err)

#State update
pos_prev = self._pos
self._pos = pos_prev + self._alpha*(zi - pos_prev)
self._vel = self._vel + self._beta*((zi - pos_prev)/self._dt)
self._acc = self._acc + self._gamma*((zi - pos_prev) / (0.5*self._dt**2))

#Covariance update
self._pos_est_err = (1-self._alpha)*self._pos_est_err
self._vel_est_err = (1-self._beta)*self._vel_est_err
self._acc_est_err = (1-self._gamma)*self._acc_est_err

return self._pospg.init()

w,h = 1600,800

background = pg.display.set_mode((w,h))
surf = pg.surfarray.pixels3d(background)
running = True
clock = pg.time.Clock()

kalman_score = 0
reg_score = 0
iters = 0
count = 0

while running:
target = Target(background, 32)
missile = Projectile(background)
k_miss = Projectile(background,Kalman()) #kommenter inn denne linjen naar Kalman er implementert
last_x_pos = target.noisy_x_pos
noisy_draw = np.zeros((w,20))

trial = True
iters += 1

while trial:

# Setter en maksimal framerate på 300. Hvis dere vil øke denne er dette en mulig endring
clock.tick(30000)
fps = clock.get_fps()

for e in pg.event.get():
if e.type == pg.QUIT:
running = False

background.fill(0x448844)
surf[:,0:20,0] = noisy_draw

last_x_pos = target.noisy_x_pos()
# print(last_x_pos)

target.move()
missile.move(last_x_pos)
k_miss.move(last_x_pos) #kommenter inn denne linjen naar Kalman er implementert

pg.draw.rect(background, (255, 200, 0), missile.rect)
pg.draw.rect(background, (0, 200, 255), k_miss.rect) #kommenter inn denne linjen naar Kalman er implementert
pg.draw.rect(background, (255, 200, 255), target.rect)

noisy_draw[int(last_x_pos):int(last_x_pos)+20,:] = 255
noisy_draw -= 1
np.clip(noisy_draw, 0, 255, noisy_draw)

coll = missile.rect.colliderect(target.rect)
k_coll = k_miss.rect.colliderect(target.rect) #kommenter inn denne linjen naar Kalman er implementert#

if coll:
reg_score += 1

if k_coll:    #kommenter inn denne linjen naar Kalman er implementert
kalman_score += 1

oob = missile.rect.y < 20

if oob or coll or k_coll: #endre denne sjekken slik at k_coll ogsaa er med naar kalman er implementert
trial = False

pg.display.flip()

print('kalman score: ', round(kalman_score/iters,2)) #kommenter inn denne linjen naar Kalman er implementert
print('regular score: ', round(reg_score/iters,2))
count += 1
print("Nr. of tries:", count)

pg.quit()

'''class Kalman(): def __init__(self): self._dt = 1 # time step self._pos = 785 # initialized position self._vel = 1 #initialized velocity self._acc = 0.05 #initialized accelaration self._pos_est_err = 1000000 #position estimation uncertainty self._vel_est_err = 1000000 #velocity estimation uncertainty self._acc_est_err = 2000 #acceleratione stimation uncertainty self._pos_mea_err = 300 #measurement error self._vel_mea_err = 100 #velocity measurement error self._acc_mea_err = 4 #acceleration measurement error self._alpha = 0 # converges to 0.32819526927045667 self._beta = 0 # converges to 0.18099751242241782 self._gamma = 0 # gamma self._q = 30 # process uncerrainty for position self._q_v = 4 # process uncertainty for velocity self._q_a = 2 # process uncertainty for acceleration self._last_pos = 785 self._measured_velocity = 0 def calc_next(self, zi): self._measured_velocity = zi - self._last_pos self._last_pos = zi #State extrapolation self._pos = self._pos + self._vel*self._dt + 0.5*self._acc*self._dt**2 self._vel = self._vel + self._acc*self._dt self._acc = self._acc #Covariance extrapolation self._pos_est_err = self._pos_est_err + (self._dt**2 * self._vel_est_err + self._q) self._vel_est_err = self._vel_est_err + self._q_v self._acc_est_err = self._acc_est_err + self._q_a #Alhpa-beta-gamma update self

## 1. What is a Kalman filter?

A Kalman filter is a mathematical algorithm used to estimate the state of a system by combining measurements from multiple sources. It is commonly used in control systems and signal processing applications to improve the accuracy of estimates.

## 2. How does a Kalman filter work?

A Kalman filter works by using a series of measurements and a mathematical model of the system to estimate the true state of the system. It uses a prediction step to estimate the state based on the previous state and a measurement step to update the estimate based on new measurements. The filter also takes into account the uncertainty of the measurements and the system model to produce a more accurate estimate.

## 3. Why is a Kalman filter useful for tracking a moving object?

A Kalman filter is useful for tracking a moving object because it can handle noisy measurements and can predict the future state of the object based on its previous state and the system model. This allows for a more accurate and smoother estimation of the object's position, velocity, and other parameters.

## 4. What are the limitations of a Kalman filter?

One limitation of a Kalman filter is that it assumes the system being modeled is linear and the measurements are normally distributed. This may not always be the case in real-world applications, leading to less accurate estimates. Additionally, the filter may struggle to handle sudden changes or non-uniform motion in the tracked object.

## 5. How can a Kalman filter be implemented in one dimension to track a moving object?

In one dimension, a Kalman filter can be implemented by defining the state of the system as the position and velocity of the moving object. The measurement step would involve taking in measurements of the object's position, and the prediction step would use the previous state and the system model to estimate the current state. The filter would then update the state and repeat the process with each new measurement, producing a more accurate estimate of the object's position and velocity over time.