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))
