import?numpy??as?np
from?numpy?import?*
import?matplotlib.pyplot?as?plt?#導(dǎo)入圖像庫
plt.rcParams[‘font.sans-serif‘]=[‘Simhei‘]?????#解決中文顯示問題，目前只知道黑體可行
plt.rcParams[‘a(chǎn)xes.unicode_minus‘]=False???????#解決負(fù)數(shù)坐標(biāo)顯示問題

def?vari（listavg）:
????“““利用list?和均值計算方差“““
????var1=0
????for?i?in?list:
????????var1+=（i-avg）**2
????var2=（math.sqrt（var1/（len（list）-1）））
????return?var2




###第二問運動系統(tǒng)
#參數(shù)初始化
L=5000???#時間步
O=mat（[[1100][0100][0011][0001]]）
TL=mat（[[0.50][10][00.5][01]]）
Q=mat（[[0.010][00.01]]）
R=0.02
#分配空間
I=mat（[[1000][0100][0010][0001]]）
X=[mat（zeros（（41））?）]*（L+1）????#真值
X[0]=mat（[01.001.5]）.T
XGUR=[mat（zeros（（41））?）]*（L+1）??#估計值
XGUR[0]=X[0]
XGUL=[mat（zeros（（41））?）]*（L+1）?#中間估計值
Z=[mat（zeros（（11））?）]*（L+1）????#觀測值
PR=[mat（zeros（（44））?）]*（L+1）???#協(xié)方差矩陣
PR[0]=I
PL=[mat（zeros（（44））?）]*（L+1）
K=[mat（zeros（（41））?）]*（L+1）????#增益矩陣
H=[mat（zeros（（14））?）]*（L+1）
w=[mat（zeros（（21））?）]*（L+1）
v=np.random?.normal（00.02L+1?）
#定義函數(shù)，導(dǎo)函數(shù)
def?h（xx）:
????hh=arctan（xx[2]/xx[0]）
????return?hh

def?hx（XX）:
????x1=XX[0]*mat（[0010]）
????x2=（-XX[2]）*mat（[1000]）
????x3=1+（XX[2]/XX[0]）**2
????x4=XX[0]**2
????x=（1/（x3*x4））*np.add（x1x2）
????return?x
for?k?in?range（1L+1）:??????#循環(huán)
????w[k?-?1][0]?=?np.random.normal（0?0.01）
????w[k?-?1][1]?=?np.random.normal（0?0.01）
????X[k]=np.add（O*X[k-1]TL*w[k-1]）???#狀態(tài)方程
????PL[k]=np.add（O*PR[k-1]*O.T?TL*Q*TL.T?）
????XGUL[k]=O*XGUR[k-1]
????H[k]=hx（XGUL[k]）
????Z[k]=H[k]*X[k]+（h（XGUL[k]）-H[k]*XGUL[k]）+v[k]
????K[k]=PL[k]*H[k].T/（H[k]*PL[k]*H[k].T+R）
????XGUR[k]=XGUL[k]+K[k]*（Z[k]-h（XGUL[k]））
????PR[k]=np.add（I-1*K[k]*H[k]）*PL[k]

fig1=plt.figure（figsize=?（88））
ax1=fig1.add_subplot?（221）
ax2=fig1.add_subplot?（222）
ax3=fig1.add_subplot?（223）
ax4=fig1.add_subplot?（224）
x=[]
vx=[]
y=[]
vy=[]

xg=[]
vxg=[]
yg=[]
vyg=[]

fig2=plt.figure（figs