Java 卡尔曼滤波器。全球定位系统&x2B；加速计

我知道网上有很多文章。但我不能把我的头绕在它周围。 我请求你的帮助

我正在使用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();
}

这是。也许这会有所帮助

谢谢