"" "
2019.11.16
Kalman Filter - the ASN course
Zhaoxueqi
"" "
The import numpy as np
The import matplotlib. Pyplot as PLT
Def kf (Z, x, P, H, R, u, A, Q) :
K_temp=K=0
List_x=[]
List_y=[]
For I in range (50) :
K_temp=K
X=A * x
P=A * * A.T P + Q
K=P * * np H.T. Linalg. Inv (H * * H.T P + R)
If (K_temp==K) :
Break
X=x + K * (Z - H * x)
P=(np) eye (Keith hape [0]) - K * H) * P
Y=I
List_x. Append (x)
List_y. Append (y)
PLT. The plot (y, x)
Print (list_x)
Print (list_y)
PLT. The show ()
A=np. Matrix ([1])
H=np. Matrix ([1])
U=0
X=0.37842
Z=H * x + np. Random. Normal (0, 0.01, 1) # Zk having noise with N (0, 4), H=1
R=0.81
P=1
Q=0
Kf (Z, x, P, H, R, u, A, Q)
CodePudding user response:
First you need to find a can be a bitmap graphics library operation, and then write the color value of each pixel in lattice way