kalman滤波
import matplotlib.pyplot as plt
from math import pi
import numpy as np
from numpy import log, exp, sqrt
from numpy.random import normal, randn
def kalman_filter_for_weiner_process(t, X):
''''X is an array'''
n_samples = len(X)
_size = n_samples + 5
sigma2 = 10 # difussion coef
Q = 10 # drift noise variation
K = np.zeros(_size)
P = np.ones([_size,_size]) # P0 = 1
λ_hat = np.zeros(_size) # a0 = 1
y_pred = np.zeros(_size)
for i in np.arange(0, n_samples):
if i == 0:
y_pred[i] = X[i]
else:
# Prediction (expectation)
P[i][i-1] = P[i-1][i-1] + Q
K[i] = (t[i] - t[i-1])**2 * P[i][i-1] + sigma2 * (t[i] - t[i-1]) # kalman 增益
λ_hat[i] = λ_hat[i-1] + P[i][i-1] * (t[i] - t[i-1]) * (X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1])) / K[i]
# Correction (variance)
P[i][i] = P[i][i-1] - P[i][i-1] * (t[i] - t[i-1])**2 / K[i] * P[i][i-1]
# 基于更新后的λ_hat进行下一步的预测
y_pred[i] = y_pred[i-1] + λ_hat[i] * (t[i] - t[i-1])
print(f"λ_hat:{λ_hat[i]},P:{P[i][i]}")
plt.scatter(t,X, color = 'blue')
plt.plot(t,X, color = 'blue', label = 'actual')
plt.plot(t, y_pred[:len(t)], color = 'red', label='kalman')
plt.scatter(t, y_pred[:len(t)], color = 'red')
print(y_pred)
print(λ_hat)
print(X)
plt.legend()
# data
t = np.arange(1,15)
X = np.sin(np.arange(1,15)) * 100
kalman_filter_for_weiner_process(t, X)
改进的kalman滤波
import matplotlib.pyplot as plt
from math import pi
import numpy as np
from numpy import log, exp, sqrt
from numpy.random import normal, randn
def strong_tracking_kalman_filter_for_weiner_process(t, X):
''''X is an array'''
n_samples = len(t)
_size = n_samples + 5
print("n_samples:", n_samples)
sigma2 = 10 # difussion coef
Q = 10 # drift noise variation
alpha, rho = 0.5, 0.5
V = np.zeros(_size)
gamma = np.zeros(_size)
B = np.zeros(_size)
C = np.zeros(_size)
v = np.zeros(_size)
K = np.zeros(_size)
P = np.ones([_size,_size]) # P0 = 1
λ_hat = np.zeros(_size) # a0 = 0
y_pred = np.zeros(_size)
for i in np.arange(0, n_samples):
if i == 0:
y_pred[i] = X[i]
else:
gamma[i] = X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1])
if i == 1:
V[i] = gamma[i]**2
elif i > 1:
V[i] = (rho * V[i-1] + gamma[i]**2) / (1 + rho)
B[i] = V[i] - Q * (t[i] - t[i-1])**2 - alpha * sigma2 * (t[i] - t[i-1])
C[i] = P[i-1][i-1] * (t[i] - t[i-1])**2 - alpha * sigma2 * (t[i] - t[i-1])
v0 = B[i] / C[i]
if v0 >= 1:
v[i] = v0
else:
v[i] = 1
# Prediction (expectation)
P[i][i-1] = v[i] * P[i-1][i-1] + Q
K[i] = (t[i] - t[i-1])**2 * P[i][i-1] + sigma2 * (t[i] - t[i-1]) # kalman 增益
λ_hat[i] = λ_hat[i-1] + P[i][i-1] * (t[i] - t[i-1]) / K[i] * (X[i] - X[i-1] - λ_hat[i-1] * (t[i] - t[i-1]))
# Correction (variance)
P[i][i] = P[i][i-1] - P[i][i-1] * (t[i] - t[i-1])**2 / K[i] * P[i][i-1]
# 基于调整过后的λ_hat进行下一步的预测
y_pred[i] = y_pred[i-1] + λ_hat[i] * (t[i] - t[i-1])
print(f"λ_hat:{λ_hat[i-1]},P:{P[i][i]}")
plt.plot(t, X, color = 'blue', label = 'actual')
plt.scatter(t, X, color = 'blue')
plt.plot(t, y_pred[:len(t)], color = 'red', label='STF')
plt.scatter(t, y_pred[:len(t)], color = 'red')
print("y_pred:",y_pred)
print("λ_hat:",λ_hat)
print("X:",X)
plt.legend()