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
def touslesamers(): sigmaalpha=me.sigmaalpha sigmarho=me.sigmarho Z0=me.mesureRelativeDesAmeres(actu.vec[0],m.ameres) P = [0.]*(3+np.size(Z0)); for i in range(0,3+np.size(Z0)): P[i] = [0.]*(3+np.size(Z0)); P = pre.toMatrix(P); J = [0.]*(np.size(Z0)/2); R = pre.toMatrix([[np.power(sigmarho,2) , 0],[0 , np.power(sigmaalpha,2)]]); for i in range(0,np.size(Z0)/2): J[i] = pre.toMatrix([[np.cos(Z0[2*i+1]) , np.sin(Z0[2*i+1])],[-Z0[2*i]*np.sin(Z0[2*i+1]) , Z0[2*i]*np.cos(Z0[2*i+1])]]); Q = J[i]*R*np.transpose(J[i]); P[3+2*i,3+i*2] = Q[0,0]; P[3+2*i,4+i*2] = Q[0,1]; P[4+2*i,3+i*2] = Q[1,0]; P[4+2*i,4+i*2] = Q[1,1]; maj.MatricePkk[0]=P
def init(x0,y0,teta0): vec.append([x0,y0,teta0]); vecbruit.append([x0,y0,teta0]); vecKalman.append(vec[0]+ me.mesureRelativeDesAmeres(vec[0],m.ameres))