phi_11=1.0 phi_12=TS phi_21=F21*TS phi_22=1.0+F22*TS PHI = np.matrix([[phi_11, phi_12],[phi_21, phi_22]]) q_11=PHIS*TS*TS*TS/3.0 q_12=PHIS*(TS*TS/2.0+F22*TS*TS*TS/3.0) q_21=q_12 q_22=PHIS*(TS+F22*TS*TS+F22*F22*TS*TS*TS/3.0) Q = np.matrix([[q_11, q_12],[q_21, q_22]]) M=PHI*P*PHI.transpose()+PHIS*Q K = M*HMAT.transpose()*(inv(HMAT*M*HMAT.transpose() + R)) P=(I-K*HMAT)*M XNOISE = GAUSS_PY(SIGMA_NOISE) XDB= XDH+TS*f2(XH,XDH,T) XB = XH+TS*XDB XS = X1+XNOISE RES= XS-XB XH=XB+K[0,0]*RES XDH=XDB+K[1,0]*RES ERRX1=X1-XH SP11=math.sqrt(P[0,0]) SP11N = -SP11 ERRX1D=X1D-XDH SP22=math.sqrt(P[1,1]) SP22N = -SP22 t.append(T) res.append(RES) x1.append(X1)
YTDD = -G
XT = XT + H * XTD
XTD = XTD + H * XTDD
YT = YT + H * YTD
YTD = YTD + H * YTDD
T = T + H
XTDD = 0.
YTDD = -G
XT = .5 * (XTOLD + XT + H * XTD)
XTD = .5 * (XTDOLD + XTD + H * XTDD)
YT = .5 * (YTOLD + YT + H * YTD)
YTD = .5 * (YTDOLD + YTD + H * YTDD)
S = S + H
if (S >= (TS - .00001)):
if NOISE == 1:
THETNOISE = GAUSS_PY(SIGTH)
RTNOISE = GAUSS_PY(SIGR)
else:
THETNOISE = 0.0
RTNOISE = 0.0
S = 0.
if RK == 0:
# This routine propagates the previous estimates to the a-priori estimates using the fundamental matrix.
# This can be done because higher exponentials of the fundamental matrix is zero, i.e. F*F = F*F*F = F*F*F*F = 0
#XTB=XTH+TS*XTDH
#XTDB=XTDH
#YTB=YTH+TS*YTDH-.5*G*TS*TS
#YTDB=YTDH-G*TS
XTB = XHMAT[0, 0] + TS * XHMAT[1, 0]
XTDB = XHMAT[1, 0]
YTB = XHMAT[2, 0] + TS * XHMAT[3, 0] - .5 * G * TS * TS
TS = 0.1
SIGNOISE = 1.
A0 = 1.
A1 = 0.
XH = 0.
XN = 0
L = int(10 / TS)
xherr = []
xs = []
xh = []
t = []
sp11 = []
act = []
# Generate the noise list so it is possible to test the same fortran code
XNOISE_g = [GAUSS_PY(SIGNOISE) for k in range(0, L)]
for k in range(0, L):
ACT = A0 + A1 * TS * k
XS = ACT + XNOISE_g[k] # signal plus noise
K_f = 1. / (k + 1) # gain of filter
RES = XS - XH # residuals
XH = XH + K_f * RES # x_k predicted
SP11 = SIGNOISE / math.sqrt(k + 1)
XHERR = ACT - XH
EPS = 0.5 * A1 * TS * (k)
xherr.append(XHERR)
xs.append(XS)
xh.append(XH)
t.append(TS * k)
sp11.append(SP11)
#STEP 8: Estimate the measurements using the H matrix
YS_LIST = [HMAT * XS for XS in XS_LIST_NEW]
#STEP 9: Estimate the weighted measurements
YH = sum([W * Y for W, Y in zip(W_LIST, YS_LIST)])
#STEP 10: Estimate new variance
S_LIST = [(YS - YH) * (YS - YH).transpose() for YS in YS_LIST]
#STEP 11: Estimate the weighted S Matrix
SMAT = sum([W * S for W, S in zip(W_LIST, S_LIST)]) + RMAT
#STEP 12 and STEP 13: Estimate the cross covariance between x and y
PXY_LIST = [(XS - XBAR) * (YS - YH)
for XS, YS in zip(XS_LIST_NEW, YS_LIST)]
PXY = sum([W * PXY for W, PXY in zip(W_LIST, PXY_LIST)])
#STEP 14: Estimate Kalman gain
KPZ = PXY * inv(SMAT)
if NOISE == 1:
XNOISE = GAUSS_PY(SIGX)
else:
XNOISE = 0.0
XMEASU = XT + XNOISE
RESK = XMEASU - YH[0, 0]
XHAT = XBAR + KPZ * RESK
P = M - KPZ * SMAT * KPZ.transpose()
WH = XHAT[2, 0]
if (WH < 0):
WH = abs(WH)
ERRW = W - WH
SP33 = math.sqrt(P[2, 2])
ArrayT.append(T)
ArrayW.append(W)
ArrayWH.append(WH)
ArrayERRW.append(ERRW)
F21 = -32.2 * RHOH * XDH * XDH / (44000. * BETAH)
F22 = RHOH * 32.2 * XDH / BETAH
F23 = -RHOH * 32.2 * XDH * XDH / (2. * BETAH * BETAH)
F = np.matrix([[0, 1, 0], [F21, F22, F23], [0, 0, 0]])
else:
if (CHOICE == 1):
F22 = (32.2 * RHOH * XDH * XDH / (2. * BETAH) - BETAH) / XDH
F = np.matrix([[0, 1, 0], [0, F22, 1], [0, 0, 0]])
else:
F22 = (32.2 * RHOH * XDH * XDH / (2. * BETAH) + BETAH) / XDH
F = np.matrix([[0, 1, 0], [0, F22, -1], [0, 0, 0]])
PHI = IDNP + TS * F
M = PHI * P * PHI.transpose() + Q
K = M * HMAT.transpose() * inv(HMAT * M * HMAT.transpose() + RMAT)
P = (IDNP - K * HMAT) * M
XNOISE = GAUSS_PY(SIGNOISE)
[XB, XDB] = projectc18l2(T, TS, XH, XDH, BETAH, HP)
RES = X + XNOISE - XB
XH = XB + K[0, 0] * RES
XDH = XDB + K[1, 0] * RES
BETAH = BETAH + K[2, 0] * RES
ERRX = X - XH
SP11 = math.sqrt(P[0, 0])
ERRXD = XD - XDH
SP22 = math.sqrt(P[1, 1])
ERRBETA = BETA - BETAH
SP33 = math.sqrt(P[2, 2])
SP11P = -SP11
SP22P = -SP22
SP33P = -SP33
ArrayT.append(T)
# Calculate cross covariance between propagated state estimates and measurement (STEP 12) Pxy0 = (XS0 - XBAR) * (YS0 - YH).transpose() Pxy1 = (XS1 - XBAR) * (YS1 - YH).transpose() Pxy2 = (XS2 - XBAR) * (YS2 - YH).transpose() Pxy3 = (XS3 - XBAR) * (YS3 - YH).transpose() Pxy4 = (XS4 - XBAR) * (YS4 - YH).transpose() Pxy5 = (XS5 - XBAR) * (YS5 - YH).transpose() Pxy6 = (XS6 - XBAR) * (YS6 - YH).transpose() Pxy7 = (XS7 - XBAR) * (YS7 - YH).transpose() Pxy8 = (XS8 - XBAR) * (YS8 - YH).transpose() # Form weighted average (STEP 13) Pxy = W0 * Pxy0 + W1 * Pxy1 + W2 * Pxy2 + W3 * Pxy3 + W4 * Pxy4 + W5 * Pxy5 + W6 * Pxy6 + W7 * Pxy7 + W8 * Pxy8 # Kalman filter (STEP 14) K = Pxy * inv(SMAT) #compute Kalman gain P = M - K * SMAT * K.transpose() #update covariance with Kalman gain THNOISE = GAUSS_PY(SIGTH) RNOISE = GAUSS_PY(SIGR) TH = math.atan2(YT - YR, XT - XR) R = math.sqrt((XT - XR)**2 + (YT - YR)**2) THMEAS = TH + THNOISE RMEAS = R + RNOISE RESTH = THMEAS - YH[0, 0] RESR = RMEAS - YH[1, 0] XTH = XTB + K[0, 0] * RESTH + K[0, 1] * RESR XTDH = XTDB + K[1, 0] * RESTH + K[1, 1] * RESR YTH = YTB + K[2, 0] * RESTH + K[2, 1] * RESR YTDH = YTDB + K[3, 0] * RESTH + K[3, 1] * RESR ERRYD = YTD - YTDH SP44 = math.sqrt(P[3, 3]) count = count + 1 ArrayT.append(T)
