def MatriceRk():
Ameres=m.ameres
matrice=[]
for i in range(2*len(Ameres)):
matrice.append(2*len(Ameres)*[0])
for i in range(len(Ameres)):
matrice[2*i][2*i]=me.sigmarho**2
matrice[2*i+1][2*i+1]=me.sigmaalpha**2
return pre.toMatrix(matrice)
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 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
def MatriceHk():
Xk=actu.vecKalman[-2]
Ameres=m.ameres
taille=len(Xk)+len(Ameres)
matrice=[]
matrice.append([])
xk=Xk[0]
yk=Xk[1]
for i in range(len(Ameres)):
matrice[-1].append((xk-Ameres[i][0])/(np.sqrt((xk-Ameres[i][0])**2+(yk-Ameres[i][1])**2)))
#matrice[-1].append(((Ameres[i][1]-yk)/(Ameres[i][0]-xk)**2)/(1+((Ameres[i][1]-yk)/(Ameres[i][0]-xk))**2))
matrice[-1].append((Ameres[i][1]-yk)/(((Ameres[i][0]-xk)**2)+((Ameres[i][1]-yk)**2)))
matrice.append([])
for i in range(len(Ameres)):
matrice[-1].append((yk-Ameres[i][1])/(np.sqrt((xk-Ameres[i][0])**2+(yk-Ameres[i][1])**2)))
#matrice[-1].append((1/(Ameres[i][0]-xk))/(1+((Ameres[i][1]-yk)/(Ameres[i][0]-xk))**2))
matrice[-1].append(-(Ameres[i][0]-xk)/(((Ameres[i][0]-xk)**2)+((Ameres[i][1]-yk)**2)))
matrice.append([])
for i in range(len(Ameres)):
matrice[-1].append(0)
matrice[-1].append(-1)
for i in range(len(Ameres)):
matrice.append(len(Ameres)*2*[0])
matrice.append(len(Ameres)*2*[0])
for i in range(len(Ameres)):
indiceX=3+2*i
indiceY=2*i
matrice[indiceX][indiceY]=((xk-Ameres[i][0])/(np.sqrt((xk-Ameres[i][0])**2+(yk-Ameres[i][1])**2)))
matrice[indiceX][indiceY+1]=-((Ameres[i][1]-yk)/(((Ameres[i][0]-xk)**2)+((Ameres[i][1]-yk)**2)))
matrice[indiceX+1][indiceY]=(yk-Ameres[i][1])/(np.sqrt((xk-Ameres[i][0])**2+(yk-Ameres[i][1])**2))
matrice[indiceX+1][indiceY+1]=(Ameres[i][0]-xk)/(((Ameres[i][0]-xk)**2)+((Ameres[i][1]-yk)**2))
return pre.toMatrix(matrice)
