Linear Kalman Filter sample

lkf.py

# -*- coding: utf-8 -*-

import numpy as np
import matplotlib.pyplot as plt

def lkf(T, Y, U, _X0, Sigma0, A, B, C, Q, R):
    '''Linear Kalman Filter
    - 状態方程式
        X(t+1) = A*X(t) + B*U(t) + V(t), V(t) ~ N(0,Q)
    - 観測方程式
        Y(t) = C*X(t) + W(t), W(t) ~ N(0,R)
    
    Parameters
    ==========
    - T : ステップ数
    - Y(t) : 観測列
    - U(t) : 入力列
    - _X(0) : 初期状態推定値
    - Sigma0 : 初期誤差共分散行列
    - A, B, C, Q, R : 状態空間モデルにおける各係数
    
    Returns
    =======
    - X(0:T) : 状態推定値列
    '''

    _X = _X0 # 初期状態推定値
    Sigma = Sigma0 # 初期誤差共分散行列

    X0_T = [_X] # 状態推定値列

    for t in range(T):
        # Prediction
        _X_ = A*_X + B*U[t] # 事前状態推定値
        Sigma_ = B*Q*B.T + A*Sigma*A.T # 事前誤差共分散行列

        # Update
        Yi = Y[t+1] - C*_X_
        Sigmai = C*Sigma_* C.T + R
        G = Sigma_*C.T*Sigmai.I
        _X = _X_+G*Yi
        Sigma = Sigma_ - G*C*Sigma_
        X0_T.append(_X)

    return X0_T

def main():
    # 状態方程式
    # X(t+1) = A*X(t) + B*U(t) + V(t), V(t) ~ N(0,Q)
    A = np.mat([[1,0], [0,1]])
    B = np.mat([[1,0], [0,1]])
    Q = np.mat([[1,0], [0,1]])
    # 観測方程式
    # Y(t) = C*X(t) + W(t), W(t) ~ N(0,R)
    C = np.mat([[1,0], [0,1]])
    R = np.mat([[2,0], [0,2]])

    # 観測のテストデータの生成
    T = 20 # 観測数
    x = np.mat([[0],[0]]) # 初期位置
    X = [x] # 状態列
    Y = [x] # 観測列
    u = np.mat([[2],[2]]) # 入力（一定）
    U = [u] # 入力列
    for i in range(T):
        x = A * x + B * u + np.random.multivariate_normal([0, 0], Q, 1).T
        X.append(x)
        y = C * x + np.random.multivariate_normal([0, 0], R, 1).T
        Y.append(y)
        U.append(u)

    # LKF
    _X0 = np.mat([[0],[0]]) # 初期状態推定値
    Sigma0 = np.mat([[1,0],[0,1]]) # 初期誤差共分散行列
    X0_T = lkf(T, Y, U, _X0, Sigma0, A, B, C, Q, R)

    # 描画
    a, b = np.array(np.concatenate(X,axis=1))
    plt.plot(a,b,'rs-', label="X_correct")
    a, b = np.array(np.concatenate(Y,axis=1))
    plt.plot(a,b,'g^-', label="Y (=X+N(0,R))")
    a, b = np.array(np.concatenate(X0_T,axis=1))
    plt.plot(a,b,'bo-', label="X_estimate")
    plt.legend(bbox_to_anchor=(0.75, 0.10), loc='lower left', borderaxespad=0)
    plt.axis('equal')
    plt.show()

if __name__ == '__main__':
    main()

Sample of linear Kalman filter