在Python中实现卡尔曼滤波器-这5个方程是否正确实现？

问题描述

有一个关于实现Kalman滤波器1D的任务。

目标是使蓝色方块变成粉红色方块。

蓝色方块会获取有关粉红色方块位置的数据，但该数据并不精确，有噪音。噪声数据的标准偏差为300像素。

蓝色方块使用卡尔曼滤波算法获得精确的位置并击中粉色方块。

只允许编辑“卡尔曼类”，并且我已经在该类中实现了卡尔曼滤波算法。

我使用了所描述的5个方程式，但仍然无法正常工作。

以下是实施Kalman过滤器的Kalman类（下面是完整代码）：

class Kalman():
    def __init__(self):
        self._dt = 1  # time step
        self._pos = 785 # position
        self._vel = 1 # veLocity
        self._acc = 0.05 # acceleration
        self._pos_est_err = 1000000 # position estimate uncertainty
        self._vel_est_err = 1000000 # veLocity estimate uncertainty
        self._acc_est_err = 2000 # acceleration estimate uncertainty
        self._pos_mea_err = 300
        self._vel_mea_err = 100
        self._acc_mea_err = 4
        self._alpha = 1 # converges to 0.32819526927045667
        self._beta = 1 # converges to 0.18099751242241782
        self._gamma = 0.01 # the acceleration doesn't change much
        
        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._pos

我使用“ Alpha-beta和gamma”值作为卡尔曼增益，因为物体（粉红色正方形）正在移动并改变其方向。

但是如何为协方差外推方程和协方差更新方程写方程？

我不确定我是否编写正确。

整个代码

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,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


class Kalman():
    def __init__(self):
        self._dt = 1  # time step
        self._pos = 785
        self._vel = 1
        self._acc = 0.05
        self._pos_est_err = 1000000
        self._vel_est_err = 1000000
        self._acc_est_err = 2000
        self._pos_mea_err = 300
        self._vel_mea_err = 100
        self._acc_mea_err = 4
        self._alpha = 1 # converges to 0.32819526927045667
        self._beta = 1 # converges to 0.18099751242241782
        self._gamma = 0.01 # the acceleration doesn't change much
        
        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._pos


pg.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())
    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) 

        pg.draw.rect(background,(255,200,0),missile.rect)
        pg.draw.rect(background,(0,255),k_miss.rect) 
        pg.draw.rect(background,target.rect)

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

        coll = missile.rect.colliderect(target.rect)
        k_coll = k_miss.rect.colliderect(target.rect)

        if coll:
            reg_score += 1

        if k_coll:    
            kalman_score += 1

        oob = missile.rect.y < 20

        if oob or coll or k_coll:
            trial = False

        pg.display.flip()

    print('kalman score: ',round(kalman_score/iters,2))
    print('regular score: ',round(reg_score/iters,2))
    count += 1
    print("Nr. of tries:",count)

pg.quit()

解决方法

您的协方差更新似乎正确，但是位置估计的公式有误。考虑您的过程模型是线性变换 x _k = F _k x _{k -1} （忽略噪声项），其中 x _k 是包含self._pos，self._vel和self._acc以及 F _k 是以下矩阵：

1  ∆t  ½∆t²
0   1   ∆t
0   0   1

其中∆ t 在您的代码中为self._dt。更新的协方差矩阵 P _{k | k -1} 的公式，在您的情况下是对角矩阵，是 P _{k | k -1} = F _k P _{k -1 | k -1} F _k ^T + R _k ，其中 P _{k -1 | k -1} 是先前的协方差矩阵和 R _k 是反映测量误差的对角协方差矩阵（即self._pos_mea_err，self._vel_mea_err和{ {1}}）。进行矩阵乘法时，所有 F _k 中的∆ t 项归零，因为它们不是在对角线上。您的协方差外推中应该没有∆ t 项。