Я делаю проект по фильтрации Калмана. Я должен использовать псевдодальности, исходящие от наблюдаемых спутников. Но количество просматриваемых спутников изменяется во времени, поэтому иногда могут изменяться массивы переменных Якобиана и состояний. Я не знаю, как с этим справиться, поскольку мне приходится использовать матрицу i-1 и i.
Вот код:
#______________________________________________________________________
def h(Xs,X):
"""vecteurs d'entrée : position des satellites. Les 4 premiers seront utilisés dans un premier temps
matrice de sortie : matrice[1,4] contenant les équations des pseudo distances """
H= np.array([np.sqrt((Xs[:,0]-X[0])**2+(Xs[:,1]-X[1])**2+(Xs[:,2]-X[2])**2)+c*X[3]])
print('calcul H ',H)
return H
#______________________________________________________
def jh(Xs, X):
"""vecteur en entrée :
Xs : positions [x,y,z] des satellites
X : Vecteur d'état [x,y,z,Δt] calculé à l'étape précédente
Matrice de sortie : matrice jacobienne du système d'équations cad jacobienne de dimension nbSat,4
"""
#there will be some weirdo stuff here. It's because by just writing c at the end, the output would be [array[5],array[5],array[5],scalar(c)] now the output is what I need
tmp = np.array(c)
for i in range (1,len(Xs)):
tmp=np.append(tmp,c)
return np.array([(Xs[:,0]-X[0])/np.sqrt((Xs[:,0]-X[0])**2+(Xs[:,1]-X[1])**2+(Xs[:,2]-X[2])**2),
(Xs[:,1]-X[1])/np.sqrt((Xs[:,0]-X[0])**2+(Xs[:,1]-X[1])**2+(Xs[:,2]-X[2])**2),
(Xs[:,2]-X[2])/np.sqrt((Xs[:,0]-X[0])**2+(Xs[:,1]-X[1])**2+(Xs[:,2]-X[2])**2),
tmp[:]]).T
#______________________________________________________________________
def q(bruitB):
"""prend en argument les valeurs de la variance des données captées à l'instant
retourne la matrice de covariance du bruit. Les bruits des différentes mesures ne sont pas corrélés entre eux"""
return np.eye(len(bruitB))*bruitB
#______________________________________________________________________
def predictionX(F,X):
"""retourne la prédiction du vecteur d'état"""
return F.dot(X)
#______________________________________________________________________
def predictionP(F,P,Q):
"""retourne la matrice de covariance de l'erreur prédite à partir de la précédente"""
return F.dot(P).dot(F.T)+Q
#______________________________________________________________________
def gain(P,R,J):
"""retourne de le gain de Kalman"""
"""J0 =[[ 5.99092039e-01 6.89189981e-02 7.97708531e-01 2.99792458e+08]
[ 2.18072986e-01 -2.54363881e-01 9.42201246e-01 2.99792458e+08]
[ 6.12139719e-01 7.08011156e-01 3.52143675e-01 2.99792458e+08]
[-2.54436938e-01 -7.13085558e-02 9.64456808e-01 2.99792458e+08]
[-9.20738262e-02 -9.24447876e-01 3.70025047e-01 2.99792458e+08]]"""
#return np.divide(P.dot(J),H.dot(P).dot(H.T)+R)
return (P.dot(J)/J.dot(P).dot(J.T)+R)#(J.dot(P).dot(J.T)+R)
#return np.matmul(P,J)/(np.matmul(np.matmul(J,P),J.T)+R)
#______________________________________________________________________
def estimationP(P,K,H):
"""retourne l'estimation' de l'erreur"""
return P-K.dot(H).dot(P)
#______________________________________________________________________
def estimationX(X,K,H,y):
"""retourne l'estimation du vecteur d'état"""
return X+K.dot(y-H.dot(X))
#______________________________________________________________________
x_e=np.array([0,0,0,0])
p_e=q(data[0].gps.bruitB)
Обратите внимание, что якобиантранспонируется в конце функции jh
"""matrice de transition"""
F=np.eye(4)
"""first state vector that I chose [x,y,z,Δt] """
X=np.array([0,0,0,0.00000001])#Δt = 10ns
"""model noise"""
Q=np.array([[1,0,0,0],
[0,1,0,0],
[0,0,0.01,0],
[0,0,0,0.000000001]])
"""Covariance de l'erreur P"""
P=np.array([[2*2,0,0,0],
[0,2*2,0,0],
[0,0,3*3,0],
[0,0,0,0.0000001]])
for iterator in data:
#initialisations
"""pseudo-ranges"""
y=iterator.gps.PRc
"""white noise of the pseudoranges"""
R=iterator.gps.bruitB*np.eye(len(iterator.gps.bruitB))
print(len(P))#4
x_p=predictionX(F,X)
p_p=predictionP(F,P,Q)
print('x = ',p_p)
H=jh(iterator.gps.Xsat,x_p)
print('H = ',H)
print('taille de H ', len(H))
K=gain(p_p,R,H)
Существует три версии для возвращаемого значения функции усиления (усиление). Первые две версии дают эту ошибку:
ValueError: формы (4,4) и (5,4) не выровнены: 4 (dim 1)! = 5 (dim 0)
Последний делает это:
ValueError: matmul: входной операнд 1 имеет несоответствие в своем основном измерении 0, с сигнатурой gufunc (n?, K), (k,m?) -> (n?, m?) (размер 5 отличается от 4)
Я думаю, с матрицей все в порядке, поэтому возникла бы проблема с кодом при использовании np.array. Действительно, число столбцов Иакова и число строк P одинаковы: 4. Таким образом, умножение должно быть возможным ...
[править]
Я думаю, что проблема в том, что я неправильно понимаю Калмана в этом деле. Я пытаюсь оценить положение x, y, z по псевдодальностям спутников nsat.
Но поскольку мы используем псевдодальности, я не понимаю, как мы связываем псевдодальности и x, y, z, t. Вот шаги, которые я думаю, что я делаю, и где я не понимаю
X - это матрица состояний, которая содержит [X, Y, Z, Δt] из предыдущегошаг.
F - это матрица перехода, которая представляет собой identity4, поскольку нам не требуется вывод переменных.
P - ковариацияМатрица ошибки, вычисленная на предыдущем шаге. Размер также равен 4 * 4, так как это ошибка между вычисленным [x, y, z, Δt] и реальной позицией. Тогда ничего не делать с псевдодальностями.
Q - матрица покрытия шума. Но это шум значения псевдодальности, поэтому его размер равен [nsat, nsat], верно? Или это шум реальной оценочной позиции? Но как я могу вычислить это, если у меня есть стандартная ошибка для каждого псевдодальности?
, из которой мы вычисляем:
предсказаниеследующие переменные состояния Xp = F, умноженные на X
предсказание ошибки Pp = FP Ftransposed + Q, но Q неправильного размера ...
мы вычисляем коэффициент усиления Калмана K
и вычисляем новую позицию: