Kalman filter in one dimension to track a moving object

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?
  • #1
Karagoz
52
5
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()
 
Physics news on Phys.org
  • #2
'''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.

Hot Threads

Recent Insights

Back
Top