指定Apache Commons卡爾曼濾波器二維定位的起始位置Estmation

Question

我使用apache commons數學庫的kalmanfilter實現來提高室內定位框架的準確性。我認爲，當狀態由位置（x，y）和速度（vx，vy）組成時，我已經正確設置了用於2D定位的矩陣。我使用「estimatedPosition（）」方法中的新傳入位置設置狀態「x」。該過濾器似乎工作：下面是從我的小JUnit測試其輸出調用該方法estimatePosition（）在循環中與嘲笑位置[20,20]：

• 第一遞歸：位置：{20; 20} 估價：{0,0054987503; 0,0054987503}
• ...
• 第100次遞歸：位置：{20; 20} 估計：{20,054973733; 20,054973733}

我想知道爲什麼最初的位置似乎在[0,0]。我必須在哪裏設置[20,20]的初始位置？

public class Kalman { 

    //A - state transition matrix 
    private RealMatrix A; 
    //B - control input matrix 
    private RealMatrix B; 
    //H - measurement matrix 
    private RealMatrix H; 
    //Q - process noise covariance matrix (error in the process) 
    private RealMatrix Q; 
    //R - measurement noise covariance matrix (error in the measurement) 
    private RealMatrix R; 
    //x state 
    private RealVector x; 

    // discrete time interval (100ms) between to steps 
    private final double dt = 0.1d; 
    // position measurement noise (1 meter) 
    private final double measurementNoise = 1d; 

    // constant control input, increase velocity by 0.1 m/s per cycle 
    private RealVector u = new ArrayRealVector(new double[] { 0.1d }); 
    //private RealVector u = new ArrayRealVector(new double[] { 10d }); 
    private KalmanFilter filter; 

    public Kalman(){ 
     //A and B describe the physic model of the user moving specified as matrices 
     A = new Array2DRowRealMatrix(new double[][] { 
                 { 1d, 0d, dt, 0d }, 
                 { 0d, 1d, 0d, dt }, 
                 { 0d, 0d, 1d, 0d }, 
                 { 0d, 0d, 0d, 1d } 
                }); 
     B = new Array2DRowRealMatrix(new double[][] { 
                 { Math.pow(dt, 2d)/2d }, 
                 { Math.pow(dt, 2d)/2d }, 
                 { dt}, 
                 { dt } 
                }); 
     //only observe first 2 values - the position coordinates 
     H = new Array2DRowRealMatrix(new double[][] { 
                 { 1d, 0d, 0d, 0d }, 
                 { 0d, 1d, 0d, 0d }, 
                }); 
     Q = new Array2DRowRealMatrix(new double[][] { 
                 { Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d, 0d }, 
                 { 0d, Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d }, 
                 { Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d), 0d }, 
                 { 0d, Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d) } 
                }); 

     R = new Array2DRowRealMatrix(new double[][] { 
       { Math.pow(measurementNoise, 2d), 0d }, 
       { 0d, Math.pow(measurementNoise, 2d) } 
     }); 

     ProcessModel pm = new DefaultProcessModel(A, B, Q, x, null); 
     MeasurementModel mm = new DefaultMeasurementModel(H, R); 
     filter = new KalmanFilter(pm, mm); 
    } 


    /** 
    * Use Kalmanfilter to decrease measurement errors 
    * @param position 
    * @return 
    */ 
    public Position<Euclidean2D> esimatePosition(Position<Euclidean2D> position){ 

     double[] pos = position.toArray(); 
     // x = [ 0 0 0 0] state consists of position and velocity[pX, pY, vX, vY] 
     x = new ArrayRealVector(new double[] { pos[0], pos[1], 0, 0 }); 

     // predict the state estimate one time-step ahead 
     filter.predict(u); 

     // x = A * x + B * u (state prediction) 
     x = A.operate(x).add(B.operate(u)); 

     // z = H * x (measurement prediction) 
     RealVector z = H.operate(x); 

     // correct the state estimate with the latest measurement 
     filter.correct(z); 

     //get the corrected state - the position 
     double pX = filter.getStateEstimation()[0]; 
     double pY = filter.getStateEstimation()[1]; 

     return new Position2D(pX, pY); 
    } 
} 

Answer 1

技術回答你的問題可能是設置x到初始狀態在Kalman()構造。

實際上，當你初始化一個卡爾曼濾波器時，你不會總是有一個你知道的初始狀態。在你自己的情況下，你碰巧知道初始位置是20,20，但是你應該在初始速度估計中加入什麼？

一個共同的起點是初始化爲0（或任何合理的平均值）並且將初始P設置爲「全開」。我看不到P是如何在你的代碼中初始化的。你可以說它的初始位置是0,0，具有很大的不確定性。這會導致初始測量對x進行大量調整，因爲P在多次測量後收斂於穩定狀態。