kalman filter

kalman.py

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

delta_t = 0.1
t = np.arange(0, 5, delta_t)
n = len(t)
# 加速度
g = 10
# 真实位置
x = 0.5 * g * t ** 2
# 带噪声测量值
z = x + np.sqrt(10) * np.random.randn(n)

# 环境噪声(速度)
Q = np.array([[0, 0], [0, 0.9]])
# 位置测量方差估计，影响收敛速度
R = np.array([[10.0]])

# 预测矩阵: xk = xk_1 + delta_t * vk_t
A = np.array([[1, delta_t], [0, 1]])
# 控制矩阵: x += 1/2*g*t^2, v += t*g
B = np.array([[0.5 * delta_t ** 2], [delta_t]])
# 测量矩阵(位置)
H = np.array([[1, 0]])

# 状态: 位置, 速度
xhat = np.zeros((2, n))
P = np.zeros(Q.shape)
I = np.eye(Q.shape[0])

for k in range(1, n):
    # 时间更新过程
    xhatminus = A @ xhat[:, [k-1]] + B * g
    Pminus = A @ P @ A.T + Q
    # 测量更新过程
    K = Pminus @ H.T @ (inv(H @ Pminus @ H.T + R)) # inv
    xhat[:, [k]] = xhatminus + K @ (z[k] - H @ xhatminus)
    P = (I - K @ H) * Pminus

plt.plot(t, x, 'r', label='real pos')
plt.plot(t, z, 'g', label='observation')
plt.plot(t, xhat[0, :], 'b', label='estimate')
plt.legend()
plt.show()

kalman_2d.py

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

delta_t = 0.1
ts = np.arange(0, 4, delta_t)

acc_x = np.exp(ts[::-1])
acc_y = np.exp(ts)

# xk = xk_1 + delta_t * vx_k_1
# yk = yk_1 + delta_t * vy_k_1
A = np.array([
    [1, 0, delta_t, 0],
    [0, 1, 0, delta_t],
    [0, 0, 1, 0],
    [0, 0, 0, 1],
])

# vx_k = vx_k_1 + acc_x_k_1
# vy_k = vy_k_1 + acc_y_k_1
B = np.array([
    [0, 0],
    [0, 0],
    [1, 0],
    [0, 1],
])

H = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
])

# init estimate: x, y, vx, vy
realx = x = np.array([[0, 0, 0, 0]]).T
P = np.eye(4)  # init cov of x
Q = np.eye(4) / 10.  # cov of noize
R = np.eye(2) * 10.  # cov of z
I = np.eye(4)

reals = []
obs = []
preds = []

for k in range(1, len(ts)):
    u = np.array([[acc_x[k], acc_y[k]]]).T
    realx = A @ realx + B @ u + np.random.normal(0, 1, (4, 1))
    reals.append(realx[:2])

    # predict
    x_ = A @ x + B @ u
    P_ = A @ P @ A.T + Q

    # observation
    z = H @ realx + np.random.normal(0, 10, (2, 1))
    obs.append(z)

    # update
    K = P_ @ H.T @ inv(H @ P_ @ H.T + R)
    x = x_ + K @ (z - H @ x_)
    P = (I - K @ H) * P_
    preds.append(x[:2])

reals = np.array(reals)
obs = np.array(obs)
preds = np.array(preds)
plt.plot(reals[:, 0], reals[:, 1], label='reals')
plt.plot(obs[:, 0], obs[:, 1], label='obs')
plt.plot(preds[:, 0], preds[:, 1], label='preds')
plt.legend()
plt.show()