<p>它应该可以与不同数量的卫星一起工作,没有问题。你知道吗</p>
<p>在没有任何数据的情况下调试代码有点困难。我在MATLAB中用一个简单的轨迹生成器实现了伪距离估计器(假设XYZ域中的世界是平的)。在每次迭代中,卫星的数量在4到14颗之间变化。你知道吗</p>
<p>请看一下我的密码。也许你会找到一些提示。
我会检查一下你的实现,找出一些不匹配的地方。你知道吗</p>
<pre><code>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
</code></pre>
<p>估计器工作正常:
<a href="https://i.stack.imgur.com/MYGaX.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/MYGaX.png" alt="GPS pseudo range estimator for positioning in a 3D space"/></a></p>
<p>顺便说一下,我认为你的雅可比矩阵有错误。最后一个元素应该是<code>c</code>。你知道吗</p>
<p><strong>更新</p>
<p>以下是卡尔曼滤波器的一些公式:</p>
<p>状态向量</p>
<p><a href="https://i.stack.imgur.com/U5Xlp.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/U5Xlp.gif" alt="State vector for the pseudorange estimator"/></a></p>
<p>转移矩阵是一个恒等矩阵,因为我们不改变预测步骤的状态向量</p>
<p><a href="https://i.stack.imgur.com/2sjC5.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/2sjC5.gif" alt="F matrix"/></a></p>
<p>Q是系统噪声矩阵。它的大小为<code>[n x n]</code>,其中<code>n</code>是状态向量的大小。它显示了在预测步骤中,状态增加了多少不确定性。值越小,过滤器收敛的速度就越快(但可能收敛到错误的状态)。你知道吗</p>
<p><a href="https://i.stack.imgur.com/mx07b.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/mx07b.gif" alt="Q matrix"/></a></p>
<p>每次接收器与卫星通信时,它都会获得以下信息:</p>
<p><a href="https://i.stack.imgur.com/8hP6L.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/8hP6L.gif" alt="Satillite information received by the receiver"/></a></p>
<p>有点棘手。这不是卡尔曼滤波器的测量矢量。前三列是计算测量函数和雅可比矩阵的附加信息。你知道吗</p>
<p>测量<code>y</code>(在Kalman意义上)仅由伪距组成:</p>
<p><a href="https://i.stack.imgur.com/IqGgM.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/IqGgM.gif" alt="Measurement vector with pseudoranges"/></a></p>
<p>度量函数将状态向量映射到度量。它告诉您与当前估计状态相对应的度量。这是伪距和估计状态之间的联系。你知道吗</p>
<p><a href="https://i.stack.imgur.com/QZ784.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/QZ784.gif" alt="measurement function"/></a></p>
<p>需要计算雅可比矩阵<code>H</code>,以线性化非线性测量函数<code>h</code>:</p>
<p><a href="https://i.stack.imgur.com/q7PpM.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/q7PpM.gif" alt="Jacobi matrix"/></a></p>
<p>伪距的噪声存储在矩阵<code>R</code>(而不是<code>Q</code>)中:</p>
<p><a href="https://i.stack.imgur.com/4jS9E.gif" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/4jS9E.gif" alt="pseudorange noise"/></a></p>