import pandas as pd
import matplotlib.pyplot as plt
import math
from scipy import linalg as lg
import numpy as np
F = np.array([[1,1],[0,1]])
H = np.array([[0.9,0],[0,1.1]])
Q = np.array([[0.4,0],[0,0.4]])
R = np.array([[1.2,0],[0,1.2]])
X0 = np.array([1,3])
P0 = np.array([[1.5,0],[0,1.6]])
N =200
W = np.sqrt(Q)@np.random.normal(loc=0.0, scale=1.0, size=(2,N))
V = np.sqrt(R)@np.random.normal(loc=0.0, scale=1.0, size=(2,N))
X = np.zeros((2, N))
Y = np.zeros((2, N))
X_pre = np.zeros((2, N))
X_est = np.zeros((2, N))
X_est1 = np.zeros((2, N))
Y_pre = np.zeros((2, N))
P_est = np.zeros((2,2))
P_est1 = np.zeros((N,2,2))
P_pre = np.zeros((2,2))
K = np.zeros((2,2))
y =[]
z =[]
状态方程
X[:,0]= X0 + W[:,0]for i inrange(1, N):
X[:, i]= F@X[:, i -1]+ W[:, i]
观测方程
for i inrange(0, N):
Y[:, i]= H@X[:, i]+ V[:, i]# 观测方程
plt.figure(1)
plt.plot([i for i inrange(0,N)],[X[1,i]for i inrange(0,N)],color ='red',label ='True')
plt.plot([i for i inrange(0,N)],[X_est[1,i]for i inrange(0,N)],color ='blue',label ='Filter')
plt.xlabel ='Time'
plt.title('X1 performance')
plt.legend(loc ='best')
plt.show()