def MiseAjourEtat(): MatricePk=pre.Pk() # print("zk") # print(pre.toMatrix(me.mesureRelativeDesAmeres(actu.vec[-1],m.ameres))) # print("yk") # print(MatriceYk()) # print("Xk-1") # print(actu.actu_kalman(actu.vecKalman)) # print("K") # print(MatriceKk()) # print("y") # print(MatriceYk()) actu.vecKalman[-1]=nptolist(pre.toMatrix(actu.actu_kalman(actu.vecKalman))+MatriceKk()*MatriceYk()); MatricePkk[1]=(np.identity(3+2*len(m.ameres))-MatriceKk()*MatriceHk())*pre.Pk() MatricePkk[0]=MatricePkk[1]
def MatriceYk(): T = [0]*2*len(m.ameres) for i in range(2*(len(m.ameres)-1)): T[i] = actu.vecKalman[-2][3+i] A = me.mesureRelativeDesAmeres(actu.vec[-1],m.ameres) B = me.h(actu.actu_kalman(actu.vecKalman),m.ameres) Y = [0]*len(A) for i in range(len(A)): Y [i] = A[i] - B[i] # print("ameres") # print(A[i]) # print("B") # print(B[i]) for i in range(len(Y)/2): Y[2*i+1] = me.modulopi(Y[2*i+1]) Y = pre.toMatrix(Y) return Y