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)