Difference between revisions of "Example Kalman Filter"

From Efficient Java Matrix Library
Jump to: navigation, search
 
Line 18: Line 18:
  
 
External Resources:
 
External Resources:
* [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterSimple.java KalmanFilterSimple]
+
* [https://github.com/lessthanoptimal/ejml/blob/v0.40/examples/src/org/ejml/example/KalmanFilterSimple.java KalmanFilterSimple]
* [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterOperations.java KalmanFilterOperations]
+
* [https://github.com/lessthanoptimal/ejml/blob/v0.40/examples/src/org/ejml/example/KalmanFilterOperations.java KalmanFilterOperations]
* [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterEquation.java KalmanFilterEquation]
+
* [https://github.com/lessthanoptimal/ejml/blob/v0.40/examples/src/org/ejml/example/KalmanFilterEquation.java KalmanFilterEquation]
 
* <disqus>Discuss this example</disqus>
 
* <disqus>Discuss this example</disqus>
  
Line 47: Line 47:
 
     private SimpleMatrix x,P;
 
     private SimpleMatrix x,P;
  
     @Override
+
     @Override public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
    public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
 
 
         this.F = new SimpleMatrix(F);
 
         this.F = new SimpleMatrix(F);
 
         this.Q = new SimpleMatrix(Q);
 
         this.Q = new SimpleMatrix(Q);
Line 54: Line 53:
 
     }
 
     }
  
     @Override
+
     @Override public void setState(DMatrixRMaj x, DMatrixRMaj P) {
    public void setState(DMatrixRMaj x, DMatrixRMaj P) {
 
 
         this.x = new SimpleMatrix(x);
 
         this.x = new SimpleMatrix(x);
 
         this.P = new SimpleMatrix(P);
 
         this.P = new SimpleMatrix(P);
 
     }
 
     }
  
     @Override
+
     @Override public void predict() {
    public void predict() {
 
 
         // x = F x
 
         // x = F x
 
         x = F.mult(x);
 
         x = F.mult(x);
Line 69: Line 66:
 
     }
 
     }
  
     @Override
+
     @Override public void update(DMatrixRMaj _z, DMatrixRMaj _R) {
    public void update(DMatrixRMaj _z, DMatrixRMaj _R) {
 
 
         // a fast way to make the matrices usable by SimpleMatrix
 
         // a fast way to make the matrices usable by SimpleMatrix
 
         SimpleMatrix z = SimpleMatrix.wrap(_z);
 
         SimpleMatrix z = SimpleMatrix.wrap(_z);
Line 91: Line 87:
 
     }
 
     }
  
     @Override
+
     @Override public DMatrixRMaj getState() { return x.getMatrix(); }
    public DMatrixRMaj getState() {
 
        return x.getMatrix();
 
    }
 
  
     @Override
+
     @Override public DMatrixRMaj getCovariance() { return P.getMatrix(); }
    public DMatrixRMaj getCovariance() {
 
        return P.getMatrix();
 
    }
 
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>
Line 126: Line 116:
 
     private DMatrixRMaj K;
 
     private DMatrixRMaj K;
  
     private LinearSolver<DMatrixRMaj> solver;
+
     private LinearSolverDense<DMatrixRMaj> solver;
  
     @Override
+
     @Override public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
    public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
 
 
         this.F = F;
 
         this.F = F;
 
         this.Q = Q;
 
         this.Q = Q;
Line 153: Line 142:
 
     }
 
     }
  
     @Override
+
     @Override public void setState(DMatrixRMaj x, DMatrixRMaj P) {
    public void setState(DMatrixRMaj x, DMatrixRMaj P) {
 
 
         this.x.set(x);
 
         this.x.set(x);
 
         this.P.set(P);
 
         this.P.set(P);
 
     }
 
     }
  
     @Override
+
     @Override public void predict() {
    public void predict() {
 
  
 
         // x = F x
 
         // x = F x
Line 172: Line 159:
 
     }
 
     }
  
     @Override
+
     @Override public void update(DMatrixRMaj z, DMatrixRMaj R) {
    public void update(DMatrixRMaj z, DMatrixRMaj R) {
 
 
         // y = z - H x
 
         // y = z - H x
 
         mult(H,x,y);
 
         mult(H,x,y);
Line 199: Line 185:
 
     }
 
     }
  
     @Override
+
     @Override public DMatrixRMaj getState() { return x; }
    public DMatrixRMaj getState() {
 
        return x;
 
    }
 
  
     @Override
+
     @Override public DMatrixRMaj getCovariance() { return P; }
    public DMatrixRMaj getCovariance() {
 
        return P;
 
    }
 
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>
Line 219: Line 199:
 
  * @author Peter Abeles
 
  * @author Peter Abeles
 
  */
 
  */
public class KalmanFilterEquation implements KalmanFilter{
+
public class KalmanFilterEquation implements KalmanFilter {
  
 
     // system state estimate
 
     // system state estimate
     private DMatrixRMaj x,P;
+
     private DMatrixRMaj x, P;
  
 
     private Equation eq;
 
     private Equation eq;
  
 
     // Storage for precompiled code for predict and update
 
     // Storage for precompiled code for predict and update
     Sequence predictX,predictP;
+
     Sequence predictX, predictP;
     Sequence updateY,updateK,updateX,updateP;
+
     Sequence updateY, updateK, updateX, updateP;
  
     @Override
+
     @Override public void configure( DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H ) {
    public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
 
 
         int dimenX = F.numCols;
 
         int dimenX = F.numCols;
  
         x = new DMatrixRMaj(dimenX,1);
+
         x = new DMatrixRMaj(dimenX, 1);
         P = new DMatrixRMaj(dimenX,dimenX);
+
         P = new DMatrixRMaj(dimenX, dimenX);
  
 
         eq = new Equation();
 
         eq = new Equation();
Line 241: Line 220:
 
         // Provide aliases between the symbolic variables and matrices we normally interact with
 
         // Provide aliases between the symbolic variables and matrices we normally interact with
 
         // The names do not have to be the same.
 
         // The names do not have to be the same.
         eq.alias(x,"x",P,"P",Q,"Q",F,"F",H,"H");
+
         eq.alias(x, "x", P, "P", Q, "Q", F, "F", H, "H");
  
 
         // Dummy matrix place holder to avoid compiler errors.  Will be replaced later on
 
         // Dummy matrix place holder to avoid compiler errors.  Will be replaced later on
         eq.alias(new DMatrixRMaj(1,1),"z");
+
         eq.alias(new DMatrixRMaj(1, 1), "z");
         eq.alias(new DMatrixRMaj(1,1),"R");
+
         eq.alias(new DMatrixRMaj(1, 1), "R");
  
 
         // Pre-compile so that it doesn't have to compile it each time it's invoked.  More cumbersome
 
         // Pre-compile so that it doesn't have to compile it each time it's invoked.  More cumbersome
Line 258: Line 237:
 
     }
 
     }
  
     @Override
+
     @Override public void setState( DMatrixRMaj x, DMatrixRMaj P ) {
    public void setState(DMatrixRMaj x, DMatrixRMaj P) {
 
 
         this.x.set(x);
 
         this.x.set(x);
 
         this.P.set(P);
 
         this.P.set(P);
 
     }
 
     }
  
     @Override
+
     @Override public void predict() {
    public void predict() {
 
 
         predictX.perform();
 
         predictX.perform();
 
         predictP.perform();
 
         predictP.perform();
 
     }
 
     }
  
     @Override
+
     @Override public void update( DMatrixRMaj z, DMatrixRMaj R ) {
    public void update(DMatrixRMaj z, DMatrixRMaj R) {
 
  
 
         // Alias will overwrite the reference to the previous matrices with the same name
 
         // Alias will overwrite the reference to the previous matrices with the same name
         eq.alias(z,"z"); eq.alias(R,"R");
+
         eq.alias(z, "z",R, "R");
  
 
         updateY.perform();
 
         updateY.perform();
Line 282: Line 258:
 
     }
 
     }
  
     @Override
+
     @Override public DMatrixRMaj getState() { return x; }
    public DMatrixRMaj getState() {
 
        return x;
 
    }
 
  
     @Override
+
     @Override public DMatrixRMaj getCovariance() { return P; }
    public DMatrixRMaj getCovariance() {
 
        return P;
 
    }
 
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>

Latest revision as of 21:08, 4 November 2020

Here are three examples that demonstrate how a Kalman filter can be created using different API's in EJML. Each API has different advantages and disadvantages. High level interfaces tend to be easier to use, but sacrifice efficiency. The intent of this article is to illustrate this trend empirically. Runtime performance of each approach is shown below. To see how complex and readable each approach is check out the source code below.

API Execution Time (ms)
SimpleMatrix 1875
Operations 1280
Equations 1698


External Resources:


NOTE: While the Kalman filter code below is fully functional and will work well in most applications, it might not be the best. Other variants seek to improve stability and/or avoid the matrix inversion. It's worth point out that some people say you should never invert the matrix in a Kalman filter. There are applications, such as target tracking, where matrix inversion of the innovation covariance is helpful as a preprocessing step.

SimpleMatrix Example

/**
 * A Kalman filter implemented using SimpleMatrix.  The code tends to be easier to
 * read and write, but the performance is degraded due to excessive creation/destruction of
 * memory and the use of more generic algorithms.  This also demonstrates how code can be
 * seamlessly implemented using both SimpleMatrix and DMatrixRMaj.  This allows code
 * to be quickly prototyped or to be written either by novices or experts.
 *
 * @author Peter Abeles
 */
public class KalmanFilterSimple implements KalmanFilter{

    // kinematics description
    private SimpleMatrix F,Q,H;

    // sytem state estimate
    private SimpleMatrix x,P;

    @Override public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
        this.F = new SimpleMatrix(F);
        this.Q = new SimpleMatrix(Q);
        this.H = new SimpleMatrix(H);
    }

    @Override public void setState(DMatrixRMaj x, DMatrixRMaj P) {
        this.x = new SimpleMatrix(x);
        this.P = new SimpleMatrix(P);
    }

    @Override public void predict() {
        // x = F x
        x = F.mult(x);

        // P = F P F' + Q
        P = F.mult(P).mult(F.transpose()).plus(Q);
    }

    @Override public void update(DMatrixRMaj _z, DMatrixRMaj _R) {
        // a fast way to make the matrices usable by SimpleMatrix
        SimpleMatrix z = SimpleMatrix.wrap(_z);
        SimpleMatrix R = SimpleMatrix.wrap(_R);

        // y = z - H x
        SimpleMatrix y = z.minus(H.mult(x));

        // S = H P H' + R
        SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(R);

        // K = PH'S^(-1)
        SimpleMatrix K = P.mult(H.transpose().mult(S.invert()));

        // x = x + Ky
        x = x.plus(K.mult(y));

        // P = (I-kH)P = P - KHP
        P = P.minus(K.mult(H).mult(P));
    }

    @Override public DMatrixRMaj getState() { return x.getMatrix(); }

    @Override public DMatrixRMaj getCovariance() { return P.getMatrix(); }
}

Operations Example

/**
 * A Kalman filter that is implemented using the operations API, which is procedural.  Much of the excessive
 * memory creation/destruction has been reduced from the KalmanFilterSimple. A specialized solver is
 * under to invert the SPD matrix.
 *
 * @author Peter Abeles
 */
public class KalmanFilterOperations implements KalmanFilter{

    // kinematics description
    private DMatrixRMaj F,Q,H;

    // system state estimate
    private DMatrixRMaj x,P;

    // these are predeclared for efficiency reasons
    private DMatrixRMaj a,b;
    private DMatrixRMaj y,S,S_inv,c,d;
    private DMatrixRMaj K;

    private LinearSolverDense<DMatrixRMaj> solver;

    @Override public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) {
        this.F = F;
        this.Q = Q;
        this.H = H;

        int dimenX = F.numCols;
        int dimenZ = H.numRows;

        a = new DMatrixRMaj(dimenX,1);
        b = new DMatrixRMaj(dimenX,dimenX);
        y = new DMatrixRMaj(dimenZ,1);
        S = new DMatrixRMaj(dimenZ,dimenZ);
        S_inv = new DMatrixRMaj(dimenZ,dimenZ);
        c = new DMatrixRMaj(dimenZ,dimenX);
        d = new DMatrixRMaj(dimenX,dimenZ);
        K = new DMatrixRMaj(dimenX,dimenZ);

        x = new DMatrixRMaj(dimenX,1);
        P = new DMatrixRMaj(dimenX,dimenX);

        // covariance matrices are symmetric positive semi-definite
        solver = LinearSolverFactory_DDRM.symmPosDef(dimenX);
    }

    @Override public void setState(DMatrixRMaj x, DMatrixRMaj P) {
        this.x.set(x);
        this.P.set(P);
    }

    @Override public void predict() {

        // x = F x
        mult(F,x,a);
        x.set(a);

        // P = F P F' + Q
        mult(F,P,b);
        multTransB(b,F, P);
        addEquals(P,Q);
    }

    @Override  public void update(DMatrixRMaj z, DMatrixRMaj R) {
        // y = z - H x
        mult(H,x,y);
        subtract(z, y, y);

        // S = H P H' + R
        mult(H,P,c);
        multTransB(c,H,S);
        addEquals(S,R);

        // K = PH'S^(-1)
        if( !solver.setA(S) ) throw new RuntimeException("Invert failed");
        solver.invert(S_inv);
        multTransA(H,S_inv,d);
        mult(P,d,K);

        // x = x + Ky
        mult(K,y,a);
        addEquals(x,a);

        // P = (I-kH)P = P - (KH)P = P-K(HP)
        mult(H,P,c);
        mult(K,c,b);
        subtractEquals(P, b);
    }

    @Override public DMatrixRMaj getState() { return x; }

    @Override public DMatrixRMaj getCovariance() { return P; }
}

Equations Example

/**
 * Example of how the equation interface can greatly simplify code
 *
 * @author Peter Abeles
 */
public class KalmanFilterEquation implements KalmanFilter {

    // system state estimate
    private DMatrixRMaj x, P;

    private Equation eq;

    // Storage for precompiled code for predict and update
    Sequence predictX, predictP;
    Sequence updateY, updateK, updateX, updateP;

    @Override public void configure( DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H ) {
        int dimenX = F.numCols;

        x = new DMatrixRMaj(dimenX, 1);
        P = new DMatrixRMaj(dimenX, dimenX);

        eq = new Equation();

        // Provide aliases between the symbolic variables and matrices we normally interact with
        // The names do not have to be the same.
        eq.alias(x, "x", P, "P", Q, "Q", F, "F", H, "H");

        // Dummy matrix place holder to avoid compiler errors.  Will be replaced later on
        eq.alias(new DMatrixRMaj(1, 1), "z");
        eq.alias(new DMatrixRMaj(1, 1), "R");

        // Pre-compile so that it doesn't have to compile it each time it's invoked.  More cumbersome
        // but for small matrices the overhead is significant
        predictX = eq.compile("x = F*x");
        predictP = eq.compile("P = F*P*F' + Q");

        updateY = eq.compile("y = z - H*x");
        updateK = eq.compile("K = P*H'*inv( H*P*H' + R )");
        updateX = eq.compile("x = x + K*y");
        updateP = eq.compile("P = P-K*(H*P)");
    }

    @Override public void setState( DMatrixRMaj x, DMatrixRMaj P ) {
        this.x.set(x);
        this.P.set(P);
    }

    @Override public void predict() {
        predictX.perform();
        predictP.perform();
    }

    @Override public void update( DMatrixRMaj z, DMatrixRMaj R ) {

        // Alias will overwrite the reference to the previous matrices with the same name
        eq.alias(z, "z",R, "R");

        updateY.perform();
        updateK.perform();
        updateX.perform();
        updateP.perform();
    }

    @Override public DMatrixRMaj getState() { return x; }

    @Override public DMatrixRMaj getCovariance() { return P; }
}