时间戳

首页 > 解决方案 > 仅测量一些状态空间变量时，卡尔曼滤波器中的 H（观察）矩阵

python - 仅测量一些状态空间变量时，卡尔曼滤波器中的 H（观察）矩阵

问题描述

我正在为 2D 跟踪对象实现卡尔曼滤波器。我正在测量物体的位置和速度。目前，我假设我同时拥有来自传感器的所有数据，所以我的观察矩阵 H 是 H = eye(4,4)，一个 4x4 单位矩阵。（见下面的代码）

但是，在我的最终实现中，我将在不同时间获得来自传感器的数据。因此，在某些更新循环中，我将获得速度，而在其他更新循环中，我将获得位置。在这些情况下，我将如何编写 H 矩阵？

可以写[位置循环]

[1, 0, 0, 0 ]
[0, 1, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 0, 0 ]

[速度环]

[0, 0, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 1, 0 ]
[0, 0, 0, 1 ]

请注意，我的状态空间变量是 [x, y, vx, vy]

我想知道使用这些矩阵是否意味着我的观察结果为零或类似的东西。

我可以保持协方差矩阵不变吗？我猜不是。

#Implementation of 2D filter with FilterPy.

import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
import matplotlib.pyplot as plt

# --------- PARAM -----------

dt = 0.1

v_dev = 0.3

pos_dev = 0.8

duration = 50

acceleration_noise = 0.3
# --------- MODEL ------------

transition_matrix = [[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]]

transition_covariance = np.array([
[ 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3), 0 ],
[ 0, 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3)],
[ 0.5* pow(dt, 3), 0, dt*dt, 0],
[ 0, 0.5*dt*dt*dt, 0, dt*dt]]) * acceleration_noise *acceleration_noise # A large process noise favors the measurements. ()

#Transition matrix with acceleration componentn

observation_matrix = np.eye(4, 4)

initial_state = [0, 0, 0.5, 0.5]

initial_state_covariance = [[ pos_dev*pos_dev, 0, 0 ,0],[0, pos_dev*pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]


observation_covariance =  [[pos_dev * pos_dev , 0, 0 ,0],[0, pos_dev * pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]

#-----------------------------

#---------- FAKE DATA ---------

ind = np.array( range( round(duration/dt) ) )
time = ind * dt

position = np.zeros( (2, len(ind)) ) 
position[0,:] = time
position[1,:] = 3 * np.sin(time)

noise = pos_dev * np.random.randn(2, len(ind))
noisy_pos = position + noise

vel = position[:,1:len(ind)] - position[:,0:len(ind)-1]
vel = vel / dt

vel_ind = np.zeros( (2, len(ind) -1 )  )
vel_ind[0,:] = position[0,0:len(ind)-1]
vel_ind[1,:] = position[1,0:len(ind)-1]

vel_noise = v_dev * np.random.randn(2, len(ind) - 1 )
noisy_vel = vel + vel_noise

observations = np.zeros((len(ind), 4))

observations[:,[0,1]] = np.transpose(noisy_pos)
observations[1:len(ind),[2,3]] = np.transpose(noisy_vel)
observations[0,[2,3]] = np.transpose(noisy_vel[[0,1],0] )

# KALMAN!

filtered_state_means = np.zeros((len(time), 4))
filtered_state_covariances = np.zeros( ( len(time), 4, 4) )



kf = KalmanFilter( dim_x = 4, dim_z = 4) # state space: x, y, vx, vy, measuring all

kf.x = np.array( initial_state )
kf.F = np.array( transition_matrix )
kf.H = np.array( observation_matrix )
kf.P = np.array( initial_state_covariance )
kf.Q = np.array( transition_covariance )
kf.R =np.array( observation_covariance ) #measurement covariance

for i in range(0, len(time) ):

    # Ommitting some data points

    if( i > no_gps_start and i < no_gps_end):

        # No data from gps
        kf.H = np.array( ([0, 0, 0, 0],[0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]) )
    else:
        kf.H = observation_matrix 

    kf.predict()
    kf.update(observations[i])

    filtered_state_means[i] = kf.x
    filtered_state_covariances[i] = kf.P

# Plotting everything

xmin = -2
xmax = 22
ymin = -4.3
ymax = 4.3

axisLimits = [xmin, xmax, ymin, ymax]


plt.figure(1)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.')
plt.axis( axisLimits )


plt.figure(2)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.quiver( vel_ind[0,:], vel_ind[1,:], noisy_vel[0,:], noisy_vel[1,:], angles='xy', scale_units='xy', scale=10)
plt.axis( axisLimits )


plt.figure(3)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7', zorder= 1)
plt.plot( filtered_state_means[:,0], filtered_state_means[:,1], linewidth = 1, zorder= 2)
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.', color = "#fd92f8", zorder= 0)

plt.plot( no_gps_x, no_gps_y, 'ro')

plt.show()

标签： pythonkalman-filter

解决方案

你是对的，你不允许这样修改观察矩阵。

在您的情况下，最好的解决方案是顺序卡尔曼滤波器，它是专门为处理丢失的测量而开发的。通过一系列单独的标量测量来替换测量向量。如果在某个时间点不存在一个或多个测量值，过滤器可以独立处理它们并且不会损坏。

看看 Dan Simon 的“Optimal State Estimation”第 6.1 章（你可以尝试在网上找到这本书）。他推导出了卡尔曼滤波器的替代方程，这些方程很容易实现。预测步骤保持不变，您需要修改更新步骤。

优点：

  • 您根本不需要计算逆矩阵（适用于嵌入式系统）
  • 如果您的 H 矩阵有很多零，则等效的顺序表达式非常短且计算效率高

对比：

  • R 矩阵（测量协方差）必须是对角线

推荐阅读