在使用伪距d进行Kalman滤波时，无法调整输入和输出阵列的大小

Question

我在做一个关于卡尔曼滤波的项目。我必须使用来自观测卫星的伪距。但是观察到的卫星数量会随时间变化，所以有时雅可比矩阵和状态变量的数组会发生变化。我不知道如何处理它，因为我必须使用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）的返回值有三种版本。 前两个版本给出了这个错误：

值错误：形状（4,4）和（5,4）未对齐：4（尺寸1）！=5（尺寸0）

最后一个是这样的：

ValueError:matmul:输入操作数1的核心维度0与gufunc签名不匹配（n？，k），（k，m？）->；（n？，m？）（5号与4号不同）

我认为矩阵是好的，所以会有一个问题的代码在使用np.数组. 事实上，雅各的列数和P的行数是一样的：4。所以乘法应该是可能的。。。你知道吗

[编辑]

我认为这个问题来自于我对Kalman的误解。我想从nsat卫星的伪距估计x，y，z的位置。你知道吗

但是由于我们使用伪距，我不知道如何把伪距和x，y，z，t联系起来，所以这里是我认为我在做的步骤，我不明白的地方

• X是包含上一步中的[X，Y，Z，Δt]的状态矩阵。

• F是转移矩阵，它是恒等式4，因为我们不需要变量的任何导数。

• p是在上一步中计算的误差的协方差矩阵。尺寸也是4*4，因为它是计算的[x，y，z，Δt]与实际位置之间的误差。那与伪距无关。

• Q是噪声的cov矩阵。但它是伪距值的噪声，所以它的大小是[nsat，nsat]对吗？还是真实估计位置上的噪音？但是如果我有每个伪距的误差标准，我怎么计算呢？

由此我们计算：

• 下一状态变量Xp=F乘以X的预测

• 预测误差Pp=F p F T传输+Q，但Q不是正确的大小。。。

我们计算了Kalman增益K

我们计算新的位置：

• X=Xp+K。。。等等，我们应该使用传感器的数据，但它们是伪距。如何将伪距与kalman滤波器内部的笛卡尔位置联系起来？

• p=Pp-K H Pp

Answer 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

估计器工作正常：

顺便说一下，我认为你的雅可比矩阵有错误。最后一个元素应该是c。你知道吗

更新

以下是卡尔曼滤波器的一些公式：

状态向量

转移矩阵是一个恒等矩阵，因为我们不改变预测步骤的状态向量

Q是系统噪声矩阵。它的大小为[n x n]，其中n是状态向量的大小。它显示了在预测步骤中，状态增加了多少不确定性。值越小，过滤器收敛的速度就越快（但可能收敛到错误的状态）。你知道吗

每次接收器与卫星通信时，它都会获得以下信息：

有点棘手。这不是卡尔曼滤波器的测量矢量。前三列是计算测量函数和雅可比矩阵的附加信息。你知道吗

测量y（在Kalman意义上）仅由伪距组成：

度量函数将状态向量映射到度量。它告诉您与当前估计状态相对应的度量。这是伪距和估计状态之间的联系。你知道吗

需要计算雅可比矩阵H，以线性化非线性测量函数h：

伪距的噪声存储在矩阵R（而不是Q）中：

Answer 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)) 用于计算增益