Фильтр Калмана дает странные результаты

Question

Я изучаю Kalman Filters и пытался реализовать следующую проблему в Python ( описание постановки задачи ) Мой подход:

Я смоделировал его как систему без управляющего ввода и х, тета как вектор состояния. Затем я следовал формулировке (пример грузовика) здесь , но в моем коде получаются странные результаты. Я чувствую, что мой выбор вектора состояния может быть неправильным, но я не уверен.

Может кто-нибудь помочь мне с векторами состояния и другими матрицами, необходимыми для оценки?

from numpy import identity
from numpy.linalg import inv
import numpy as np
import matplotlib.pyplot as plt


class KalmanFilter:
    def __init__(self, state_estimate, error_cov, transition_model,
                 control_model, obser_model, process_cov, obser_cov):
        self.state_estimate = state_estimate
        self.error_cov = error_cov
        self.transition_model = transition_model
        self.control_model = control_model
        self.obser_model = obser_model
        self.process_cov = process_cov
        self.obser_cov = obser_cov

    def predict(self, control_input):
        self.state_estimate = self.transition_model * self.state_estimate \
                + self.control_model * control_input

        self.error_cov = self.process_cov \
            + self.transition_model * self.error_cov * self.transition_model.T

        return self.state_estimate

    def update(self, obser):
        innovation = obser - self.obser_model * self.state_estimate
        innovation_cov = self.obser_cov \
            + self.obser_model * self.error_cov * self.obser_model.T
        kalman_gain = self.error_cov * self.obser_model.T * inv(innovation_cov)

        self.state_estimate = self.state_estimate + kalman_gain * innovation

        n = self.error_cov.shape[0]
        self.error_cov = (identity(n) - kalman_gain * self.obser_model) \
            * self.error_cov

        return self.state_estimate


class Process:
    acc_std_dev = 1
    obs_std_dev = (0.010)*(0.010)

    def __init__(self, initial_state, transition_model):
        self.state = initial_state
        self.transition_model = transition_model

    def update(self):
        rand_acc = np.random.normal(0, Process.acc_std_dev)
        G = np.array([[0.25, 1]]).T
        self.state = self.transition_model * self.state + G * rand_acc
        return self.state

    def observe(self):
        return self.state[0, 0] + np.random.normal(0, Process.obs_std_dev)

if __name__ == '__main__':
    # delta time
    dt = 1

    initial_state = np.array([[0, 0]]).T

    # Assuming perfect initial state estimate
    error_cov = np.matrix([[0, 0],
                          [0, 0]])

    transition_model = np.matrix([[1, 0],
                                 [0, (1/200)]])

    # Assuming no input control
    control_model = np.array([[0, 0]]).T

    # Making observations of the position
    obser_model = np.matrix([[1, 0]])


    G = np.array([[1, 0]]).T
    process_cov = G * G.T * Process.acc_std_dev

    kf = KalmanFilter(initial_state, error_cov, transition_model,
                      control_model, obser_model, process_cov,
                      Process.obs_std_dev**2)
    p = Process(initial_state, transition_model)

    positions = list()
    velocities = list()

    observations = list()

    positionEstimates = list()
    velocityEstimates = list()

    for i in range(0, 100):
        state = p.update()
        positions.append(state[0, 0])
        velocities.append(state[1, 0])

        obs = p.observe()
        observations.append(obs)

        kf.predict(np.array([[0, 0]]))
        estimate = kf.update(obs)

        positionEstimates.append(kf.update(obs)[0, 0])
        velocityEstimates.append(kf.update(obs)[1, 0])

    xs = np.arange(len(observations))

    # Position - State, Observations, Estimate
    pStates, = plt.plot(xs, positions, color='k')
    pObs, = plt.plot(xs, observations, color='r')
    pEstimates, = plt.plot(xs, positionEstimates, color='g')
    plt.legend((pObs, pStates, pEstimates),
               ('observations', 'true state', 'estimates'))
    plt.title("Kalman Filter - Position Observations vs Estimates")
    plt.ylabel('x', rotation=90)
    plt.xlabel('t')
    plt.show()

    # Velocity - State, Estimate
    pStates, = plt.plot(xs, velocities, color='k')
    pEstimates, = plt.plot(xs, velocityEstimates, color='g')
    plt.legend((pStates, pEstimates),
               ('true state', 'estimates'))
    plt.title("Kalman Filter - Velocity")
    plt.ylabel('ẋ', rotation=90)
    plt.xlabel('t')
    plt.show()