Question

我知道互联网上有很多文章。但我无法绕过它。我正在寻求你的帮助。

我正在使用apache.math Kalman过滤器。我读过他们的例子。但似乎他们只观察一个位置。就我而言，我希望以速度为导向。

我知道创建单独的卡尔曼滤波器是一个坏主意。但由于GPS在每个步骤上具有不同的准确度，并且我的采样率变化，因此应在每次卡尔曼迭代之间重新计算矩阵（A，B等）。另外，我试着保持温和，并在步骤=）之间保存errorMatrix和sampleEstimation

卡尔曼滤镜希望R基于我的模型 1维。所以我想知道我哪里错了。

另外告诉我是否可以在卡尔曼滤波器之间传播误差和状态矩阵

我希望R依赖于位置和速度

我的代码：

public void iterate(float accelValue, Location location, long timestamp) {

    float dt = (timestamp - this.timestamp) / 1000f;
    this.timestamp = timestamp;
    this.location = location;

    // A = [ 1, dt ]
    //     [ 0,  1 ]
    A = getStateTransitionMatrix(dt);
    // B = [ dt^2/2 ]
    //     [ dt     ]
    B = getControlMatrix(dt);
    // Q = [ dt^4/4, dt^3/2 ]  * accelNoise
    //     [ dt^3/2, dt^2   ]
    Q = getProcessCovarianceMatrix(dt);
    // u = [ accelValue ]
    u = getControlVector(accelValue);
    // z = [ position, velocity ] From GPS on each step
    z = getMeasVector(location);
    // H = [ 1, 1 ]
    H = getMeasMatrix();
    // R = [ (posAccuracy * 2) ^ 2,                0 ]  from GPS on each step
    //     [ 0                    , (velAcc * 2) ^ 2 ]
    R = getMeasCovarianceMatrix(location);

    ProcessModel processModel = new DefaultProcessModel(
            A,
            B,
            Q,
            getStateEstimationVector(), // from previous kalmanFilter. Initial is x = [ 0, 0 ]
            getErrorCovarianceMatrix()); // from previous kalmanFilter. Initial is P = [ 1, 1 ] [ 1, 1 ]
    MeasurementModel measurementModel = new DefaultMeasurementModel(H, R);
    kalmanFilter = new KalmanFilter(processModel, measurementModel);
}

预测和纠正

public void predictAndCorrect() {
    kalmanFilter.predict(u);
    kalmanFilter.correct(z);
}

简而言之，请考虑以下内容：

Kalman kalman = new Kalman();

for(;;) { // consider this as callback when data is changed.
    kalman.iterate(accelValue, location, timestamp);
    kalman.predictAndCorrect();
    kalman.proceedResults();
}

这是docs example。也许它会以某种方式帮助。

谢谢！

卡尔曼滤波器。 GPS +加速度计

0 个答案: