运动过程状态空间模型的卡尔曼滤波实现

Implementing Kalman filter for state space model of movement process(运动过程状态空间模型的卡尔曼滤波实现)

问题描述

是否可以在statsModels中实现如Bayesian Filtering and Smoothing示例3.6中所示的模型？

我可以按照提供的MatLab代码进行操作，但我不确定是否以及如何在statsModels中实现这种模型。

该示例涉及跟踪对象在2D空间中的位置。状态是四维的x=(x_1, x_2, x_3, x_4)，但我重新排列了矢量，使(x_1, x_3)表示位置，(x_2, x_4)表示两个方向上的速度。Simulated data过程由100个位置观测组成，以2x100矩阵排列Y。

import numpy as np
from scipy import linalg

# The matrices in the dynamic model are set up as follows
q, dt, s = 1, 0.1, 0.5
A = np.array([[1, dt, 0, 0],
              [0, 1, 0, 0],
              [0, 0, 1, dt],
              [0, 0, 0, 1]])
Q = q * np.array([[dt ** 3 / 3, dt ** 2 / 2, 0, 0],
                  [dt ** 2 / 2, dt, 0, 0],
                  [0, 0, dt ** 3 / 3, dt ** 2 / 2],
                  [0, 0, dt ** 2 / 2, dt]])
# Matrices in the measurement model are designed as follows
H = np.array([[1, 0, 0, 0],
              [0, 0, 1, 0]])
R = s ** 2 * np.eye(2)
# Starting values
m0 = np.array([[0, 1, 0, -1]]).T  # column vector
P0 = np.eye(4)

然后按如下方式实现该过程的卡尔曼滤波：

n = 100
m = m0
P = P0
kf_m = np.zeros((m.shape[0], n))
kf_P = np.zeros((P.shape[0], P.shape[1], n))
for k in range(n):
    m = A @ m
    P = A @ P @ A.T + Q
    S = H @ P @ H.T + R
    K = linalg.lstsq(S.T, (P @ H.T).T)[0].T
    m = m + K @ (Y[:, k, np.newaxis] - H @ m)
    P = P - K @ S @ K.T

    kf_m[:, k] = m.flatten()
    kf_P[:, :, k] = P

如果可能的话，如何在统计模型中实现此筛选器？如果数据大得多，统计模型可能会运行得更高效，并且可以对子类中的筛选器实现更平滑。

推荐答案

是的，您可以这样做；主要是将您的表示法映射到StatsModels使用的表示法/变量名。

以下是您如何执行此操作的示例：

import numpy as np
import pandas as pd  # Pandas isn't necessary, but makes some output nicer
import statsmodels.api as sm

# The matrices in the dynamic model are set up as follows
q, dt, s = 1, 0.1, 0.5
A = np.array([[1, dt, 0, 0],
              [0, 1, 0, 0],
              [0, 0, 1, dt],
              [0, 0, 0, 1]])
Q = q * np.array([[dt ** 3 / 3, dt ** 2 / 2, 0, 0],
                  [dt ** 2 / 2, dt, 0, 0],
                  [0, 0, dt ** 3 / 3, dt ** 2 / 2],
                  [0, 0, dt ** 2 / 2, dt]])
# Matrices in the measurement model are designed as follows
H = np.array([[1, 0, 0, 0],
              [0, 0, 1, 0]])
R = s ** 2 * np.eye(2)
# Starting values
m0 = np.array([[0, 1, 0, -1]]).T  # column vector
P0 = np.eye(4)


# Now instantiate a statespace model with the data
# (data should be shaped nobs x n_variables))
kf = sm.tsa.statespace.MLEModel(pd.DataFrame(Y.T), k_states=4)
kf._state_names = ['x1', 'dx1/dt', 'x2', 'dx2/dt']
kf['design'] = H
kf['obs_cov'] = R
kf['transition'] = A
kf['selection'] = np.eye(4)
kf['state_cov'] = Q

# Edit: the timing convention for initialization
# in Statsmodels differs from the the in the question
# So we should not use kf.initialize_known(m0[:, 0], P0)
# But instead, to fit the question's initialization
# into Statsmodels' timing, we just need to use the
# transition equation to move the initialization
# forward, as follows:
kf.initialize_known(A @ m0[:, 0], A @ P0 @ A.T + Q)

# To performan Kalman filtering and smoothing, use:
res = kf.smooth([])

# Then, for example, to print the smoothed estimates of
# the state vector:
print(res.states.smoothed)

这篇关于运动过程状态空间模型的卡尔曼滤波实现的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持编程学习网！