卡尔曼滤波器实现-可能存在的问题

Question

抱歉这么麻烦，但我根据数十篇文章多次检查了我的代码，但我的KF仍然无法正常工作。所谓“不正常工作”，是指KF的估计值错误。这里是Real、Noised和KF估计位置的一个漂亮的剪贴板（只有一小部分）paste 。

我的示例与我找到的每个教程都相同-我有一个位置和速度的状态向量。位置以米为单位，表示空中垂直位置。我现实世界的案例是跳伞（带降落伞）。在我生成的样本数据中，我假设我们从3000m开始，速度为10m/s。

附言：我非常确定矩阵计算是正确的-逻辑上肯定有错误。

这里是我生成的数据:

void generateData(float** inData, float** noisedData, int x, int y){
    inData[0][0]= 3000; //start position
    inData[1][0]= -10; // 10m/s velocity; minus because we assume it's falling

    noisedData[0][0]= 2998; 
    noisedData[1][0]= -10;

    for(int i=1; i<x; i++){
        inData[0][i]= inData[0][i-1] + inData[1][i-1]; 
        inData[1][i]= inData[1][i-1]; //the velocity doesn't change for simplicity's sake

        noisedData[0][i]=inData[0][i]+(rand()%6-3); //we add noise to real measurement
        noisedData[1][i]=inData[1][i]; //velocity has no noise
    }  
}

这是我的实现（矩阵初始化基于维基百科卡尔曼示例）：

int main(int argc, char** argv) {
    srand(time(NULL));

    float** inData = createMatrix(100,2); //2 rows, 100 columns
    float** noisedData = createMatrix(100,2);
    float** estData = createMatrix(100,2);

    generateData(inData, noisedData, 100, 2);

    float sampleRate=0.1; //10hz

    float** A=createMatrix(2,2);
    A[0][0]=1;
    A[0][1]=sampleRate;
    A[1][0]=0;
    A[1][1]=1;

    float** B=createMatrix(1,2);
    B[0][0]=pow(sampleRate,2)/2;
    B[1][0]=sampleRate;

    float** C=createMatrix(2,1);
    C[0][0]=1; //we measure only position
    C[0][1]=0;


    float u=1.0; //acceleration magnitude
    float accel_noise=0.2; //acceleration noise
    float measure_noise=1.5; //1.5 m standard deviation
    float R=pow(measure_noise,2); //measure covariance
    float** Q=createMatrix(2,2); //process covariance
    Q[0][0]=pow(accel_noise,2)*(pow(sampleRate,4)/4);
    Q[0][1]=pow(accel_noise,2)*(pow(sampleRate,3)/2);
    Q[1][0]=pow(accel_noise,2)*(pow(sampleRate,3)/2);
    Q[1][1]=pow(accel_noise,2)*pow(sampleRate,2);

    float** P=createMatrix(2,2); //covariance update
    P[0][0]=0;
    P[0][1]=0; 
    P[1][0]=0; 
    P[1][1]=0;

    float** P_est=createMatrix(2,2);
    P_est[0][0]=P[0][0];
    P_est[0][1]=P[0][1];
    P_est[1][0]=P[1][0];
    P_est[1][1]=P[1][1];

    float** K=createMatrix(1,2); //Kalman gain

    float** X_est=createMatrix(1,2); //our estimated state
    X_est[0][0]=3000; X_est[1][0]=10; 

    // !! KALMAN ALGORITHM START !! //
    for(int i=0; i<100; i++)
    {        
        float** temp;
        float** temp2;
        float** temp3;

        float** C_trans=matrixTranspose(C,2,1);
        temp=matrixMultiply(P_est,C_trans,2,2,1,2); //2x1
        temp2=matrixMultiply(C,P_est,2,1,2,2); //1x2
        temp3=matrixMultiply(temp2,C_trans,2,1,1,2); //1x1
        temp3[0][0]+=R;
        K[0][0]=temp[0][0]/temp3[0][0]; // 1. KALMAN GAIN
        K[1][0]=temp[1][0]/temp3[0][0];

        temp=matrixMultiply(C,X_est,2,1,1,2);
        float diff=noisedData[0][i]-temp[0][0]; //diff between meas and est

        X_est[0][0]=X_est[0][0]+(K[0][0]*diff);  // 2. ESTIMATION CORRECTION
        X_est[1][0]=X_est[1][0]+(K[1][0]*diff);

        temp=createMatrix(2,2);
        temp[0][0]=1; temp[0][1]=0; temp[1][0]=0; temp[1][1]=1;
        temp2=matrixMultiply(K,C,1,2,2,1);
        temp3=matrixSub(temp,temp2,2,2,2,2);
        P=matrixMultiply(temp3,P_est,2,2,2,2);  // 3. COVARIANCE UPDATE



        temp=matrixMultiply(A,X_est,2,2,1,2);
        X_est[0][0]=temp[0][0]+B[0][0]*u; 
        X_est[1][0]=temp[1][0]+B[1][0]*u; // 4. PREDICT NEXT STATE


        temp=matrixMultiply(A,P,2,2,2,2);
        float** A_inv=getInverse(A,2);
        temp2=matrixMultiply(temp,A_inv,2,2,2,2);
        P_est=matrixAdd(temp2,Q,2,2,2,2); // 5. PREDICT NEXT COVARIANCE


        estData[0][i]=X_est[0][0]; //just saving here for later to write out
        estData[1][i]=X_est[1][0];
    }

    for(int i=0; i<100; i++) printf("%4.2f  :  %4.2f  :  %4.2f \n", inData[0][i], noisedData[0][i], estData[0][i]); // just writing out

    return (EXIT_SUCCESS);
}

- c0dehunter

---

我不会试图回答这个问题，但是假设-m/s是正确的吗？我明白你在描述物体下落，但是这些方程式是否意识到了这一点呢？保持正数并在需要时进行减法运算是否更好呢？很好奇！ - ChiefTwoPencils

我尝试过了 - 没有区别! :) - c0dehunter

Answer 1

看起来你假设问题采用刚体模型。如果是这种情况，那么在进行过程更新以预测下一个状态时，我不会输入 u。也许我错过了什么，但是输入 u 在生成数据时没有任何作用。

换句话说，将 u 设为 +1 看起来就像你的模型假设物体应该向 +x 方向移动，因为有一个朝那个方向的输入，但是测量告诉它要往另一个方向走。因此，如果你在测量上施加了很大的权重，它会向负方向走，但是如果你在模型上施加了很大的权重，它应该朝正方向走。无论如何，基于生成的数据，我没有理由将 u 设置为除零以外的任何值。

另外，你的采样率是 0.1 Hz，但是当你生成数据时，你假设它是一秒钟，因为每个样本，位置都会按照 -10 米/秒的速度变化。

这是一个 matlab/octave 实现代码。

l    = 1000;
Ts   =  0.1;
y    =  3000; %measurement to be fed to KF
v    = -10; % METERS PER SECOND
t    = [y(1);v]; % truth for checking if its working

for i=2:l
    y(i)   = y(i-1) + (v)*Ts;
    t(:,i) = [y(i);v];          % copy to truth vector
    y(i)   = y(i) + randn;     % noise it up
end


%%%%% Let the filtering begin!

% Define dynamics
A = [1, Ts; 0, 1];
B = [0;0];
C = [1,0];

% Steady State Kalman Gain computed for R = 0.1, Q = [0,0;0,0.1]
K = [0.44166;0.79889];

x_est_post = [3000;0];

for i=2:l
    x_est_pre = A*x_est_post(:,i-1); % Process update! That is our estimate in case no measurement comes in.

    %%% OMG A MEASUREMENT! 
    x_est_post(:,i) = x_est_pre + K*(-x_est_pre(1)+y(i));
end

- Mr. Fegur

Answer 2

你正在进行很多奇怪的数组索引。

float** A=createMatrix(2,2);
A[0][0]=1;
A[0][3]=sampleRate;
A[1][0]=0;
A[1][4]=1;

如果在数组范围之外进行索引，预期的结果是什么？

- HonkyTonk

---

我不知道那些数字从哪里来。在我的代码中一切都没问题。我也在这里修复了它。 - c0dehunter