使用伪距数据进行卡尔曼滤波器时无法调整输入和输出数组的大小
Posted
技术标签:
【中文标题】使用伪距数据进行卡尔曼滤波器时无法调整输入和输出数组的大小【英文标题】：Can't adjust the size of input and output arrays while doing a Kalman Filter with pseudoranges data 【发布时间】：2019-10-29 23:44:01 【问题描述】：我正在做一个关于卡尔曼滤波的项目。我必须使用来自被观察卫星的伪距。但是观察到的卫星数量会随时间变化，因此有时雅可比和状态变量的数组会发生变化。我不知道如何处理它，因为我必须使用 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)
请注意，雅可比行列式在 func 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)
增益函数（gain）的返回值有3个版本。 前两个版本给出了这个错误：
ValueError：形状 (4,4) 和 (5,4) 未对齐：4 (dim 1) != 5 (dim 0)
最后一个是这样的：
ValueError: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 5不同于 4)
我觉得矩阵没问题，所以使用np.array的代码会有问题。事实上，Jacob 的列数和 P 的行数是相同的：4. 所以乘法应该是可能的......
[编辑]
我认为问题来自我在这种情况下对卡尔曼的误解。我正在尝试从 nsat 卫星的伪距估计 x、y、z 位置。
但是由于我们使用伪距，我不明白我们如何将伪距和 x,y,z,t 联系起来所以这里是我认为我正在做的步骤以及我不明白的地方
X 是包含上一步 [X,Y,Z,Δt] 的状态矩阵。
F 是转换矩阵，它是 identity4，因为我们不需要对变量进行任何推导。
P 是上一步中计算的误差的协方差矩阵。大小也是 4*4，因为它是计算出的 [x,y,z,Δt] 和实际位置之间的误差。那么与伪距无关。
Q 是噪声的 cov 矩阵。但它是伪距值的噪声，所以它的大小是 [nsat,nsat] 对吗？或者是真实估计位置上的噪音？但是，如果我有每个伪距的错误标准，我该如何计算呢？
我们从中计算：
预测下一个状态变量 Xp=F 乘以 X
预测误差 Pp = F P Ftransposed + Q 但 Q 大小不合适...
我们计算卡尔曼增益 K
然后我们计算新的位置：
X = Xp + K ... 等等，我们应该在这里使用来自传感器的数据，但它们是伪距。我们如何将卡尔曼滤波器内的伪距和笛卡尔位置联系起来？
P = Pp - K H Pp
【问题讨论】：
【参考方案1】：它应该适用于不同数量的卫星，没有问题。
在没有任何数据的情况下调试代码有点困难。我使用一个简单的跟踪生成器在 MATLAB 中实现了伪距估计器（假设世界在 XYZ 域中只是平坦的）。每次迭代中卫星的数量在 4 到 14 之间变化。
请看一下我的代码。也许你会找到一些提示。 我会尝试检查你的实现并找出一些不匹配的地方。
function [] = main()
c = 299792458; % speed of light
[t, PosRef, PosSatArr, clockBias] = generateData(c);
n = numel(t);
% state
X = zeros(4, 1); % X, Y, Z, clockBias
% transition matrix
F = eye(4, 4);
% covariance matrix
P = diag([100^2 100^2 100^2 1]);
% system noise
Q = diag([60 60 60 1e-12]);
sigmaR2 = 1e6;
X_arr = zeros(n, 4);
for i = 1:n
% setting the measurement
y = PosSatArr(i).PosSat;
[NSat, ~] = size(y);
R = diag(sigmaR2*ones(1, NSat));
[X, P] = prediction(X, P, Q, F); %Prediction
[X, P, ~] = update(X, P, y, R, c, NSat); %Update
X_arr(i, :) = X';
end
figure;
subplot(4,1,1);
plot([t(1) t(end)], [PosRef(1) PosRef(1)], '-');
hold on;
plot(t, X_arr(:, 1));
hold off;
grid minor;
title('Position X');
legend('Ground Truth', 'Estimation');
subplot(4,1,2);
plot([t(1) t(end)], [PosRef(2) PosRef(2)], '-');
hold on;
plot(t, X_arr(:, 2));
hold off;
grid minor;
title('Position Y');
legend('Ground Truth', 'Estimation');
subplot(4,1,3);
plot([t(1) t(end)], [PosRef(3) PosRef(3)], '-');
hold on;
plot(t, X_arr(:, 3));
hold off;
grid minor;
title('Position Z');
legend('Ground Truth', 'Estimation');
subplot(4,1,4);
plot(t, clockBias, '-');
hold on;
plot(t, X_arr(:, 4));
hold off;
grid minor;
title('Clock Bias');
legend('Ground Truth', 'Estimation');
end
function h = geth(X, y, c)
h = vecnorm(y(:, 1:3) - X(1:3)', 2, 2) + c*X(4);
end
function H = getJacobi(X, y, c, NSat)
ro = vecnorm(y(:, 1:3) - X(1:3)', 2, 2); %pseudoranges from the current state and the satellite positions
H = [-(y(:, 1:3) - X(1:3)')./ro c*ones(NSat, 1)];
end
function [t, PosRef, PosSatArr, clockBias] = generateData(c)
t=1:1000;
clockBias = 0.00001 + 0.00000001*t;
PosRef = [randn(2,1); 0]*1e4;
PosSatArr = struct;
for i=1:numel(t)
NSat = 4 + randi(10);
PosSat = [randn(NSat, 3) zeros(NSat, 1)]*1e4; % X, Y, Z, PseudoRange
%PosSat(:, 3) = abs(PosSat(:, 3)); % if you want your Z to be positive
Range = vecnorm(PosSat(:, 1:3) - PosRef', 2, 2);
PosSat(:, 4) = Range + c*clockBias(i);
PosSatArr(i).PosSat = PosSat;
end
end
function [X, P] = prediction(X, P, Q, F)
X = F*X;
P = F*P*F' + Q;
end
function [X, P, K] = update(X, P, y, R, c, NSat)
h = geth(X, y, c);
H = getJacobi(X, y, c, NSat);
Inn = y(:, 4) - h;
S = H*P*H' + R;
K = P*H'/S;
X = X + K*Inn;
P = P - K*H*P;
end
估算器工作正常：
顺便说一句，我认为您的 Jacobi 矩阵有错误。最后一个元素应该是c。
更新
这里有一些卡尔曼滤波器的公式：
状态向量
转移矩阵是一个单位矩阵，因为我们不会在预测步骤中改变状态向量
Q 是系统噪声矩阵。它的大小为[n x n]，其中n 是状态向量的大小。它显示了在预测步骤期间向状态添加了多少不确定性。值越小，过滤器收敛得越快（但可能会收敛到错误的状态）。
接收器每次与卫星通信时都会获得以下信息：
这有点棘手。这不是卡尔曼滤波器的测量向量。前三列是计算测量函数和雅可比矩阵的附加信息。
测量y（在卡尔曼意义上）仅包含伪距：
测量函数将状态向量映射到测量值。它告诉您与当前估计的状态相对应的测量结果。这是伪距和估计状态之间的联系。
需要计算Jacobi矩阵H对非线性测量函数h进行线性化：
伪距的噪声存储在矩阵R中（不在Q中）：
【讨论】：
哇非常感谢您的回答。所以我必须明确我在这里尝试的是小平方方法，只是为了确保在实施卡尔曼之前一切正常。 我对卡尔曼滤波器添加了一些解释。它能回答你最后的问题吗？ 创新计算在我的代码中存在错误。我们在这里使用扩展卡尔曼滤波器，因此应该使用测量函数计算创新，而不是像以前那样使用雅可比矩阵。现在估计很完美！ 非常感谢。它帮助我理解，但仍有一些我不明白的地方。 P 是估计的协方差。所以它应该和雅可比一样大小吧？否则不能计算HPH'。我认为 P 应该是一个诊断矩阵大小 [n,n]，其中 n 是状态向量的大小，但它不能与大小为 [5,4] 的 Jacobi 相乘。它应该。但是numpy说不行：ValueError：形状（4,4）和（5,4）未对齐：4（dim 1）！= 5（dim 0） 假设状态向量为 [n, 1]，测量值为 [m, 1]。那么 P 是 [n, n] 并且 H 是 [m, n]。 HPH' 将是 [m, n][n, n][n, m] = [m, m]【参考方案2】：好的，它可以工作，主要错误在这一行
return (P.dot(J)/J.dot(P).dot(J.T)+R)#(J.dot(P).dot(J.T)+R)
我们不能只划分一个矩阵，我们必须乘以倒置矩阵，这样才有效：
return P.dot(J.T).dot(np.linalg.pinv(J.dot(P).dot(J.T)+R))
用于计算增益
【讨论】：
以上是关于使用伪距数据进行卡尔曼滤波器时无法调整输入和输出数组的大小的主要内容，如果未能解决你的问题，请参考以下文章
卡尔曼（Kalman）滤波--卡尔曼滤波的应用： 四元数卡尔曼滤波（QKF）的C代码实现姿态解算