有一项关于实现卡尔曼滤波器1D的任务。
我们的目标是让蓝色的方块碰到粉红色的方块。
蓝色方块可以得到关于粉色方块位置的数据，但数据并不精确，很嘈杂。噪声数据的标准偏差为300像素。
蓝色方块使用卡尔曼滤波算法来获得精确的位置，并击中粉红色方块。
仅"；Kalman类"；允许编辑，并且我已经在该类中实现了卡尔曼滤波算法。
我使用了描述的5个方程式，但它仍然不能正常工作。
以下是实现卡尔曼滤波器的卡尔曼类(下面是整个代码(：
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
我用了"；α-β和γ；值作为卡尔曼增益，因为对象(粉红色的正方形(正在移动并改变其方向。
但是我如何为协方差外推方程和协方差更新方程编写方程
我不确定我写得是否正确。
整个代码
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
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, 200, 255), k_miss.rect)
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)
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()
您的协方差更新看起来是正确的，但位置估计的公式有错误。请考虑您的流程模型是线性变换xk=Fk<1>(忽略噪声项(，其中xk是包含self._pos、self._vel和self._acc的列向量，Fk是以下矩阵：
1 ∆t ½∆t²
0 1 ∆t
0 0 1
其中∆t是代码中的self._dt。更新的协方差矩阵Pk|k-1的公式，在您的情况下是对角矩阵，是Pk+|k-1=FkPk-1|k-1F，其中Pk-1|k-1是先前的协方差矩阵，Rk为反映测量误差的对角协方差矩阵(即self._pos_mea_err、self._vel_mea_err和self._acc_mea_err(。进行矩阵乘法运算时，Fk中的所有∆t项都为零，因为它们不在对角线上。协方差外推中不应有∆t项。