Difference between revisions of "Example Kalman Filter"
(9 intermediate revisions by the same user not shown) | |||
Line 1: | Line 1: | ||
− | Here are three examples that demonstrate how a [http://en.wikipedia.org/wiki/Kalman_filter Kalman filter | + | Here are three examples that demonstrate how a [http://en.wikipedia.org/wiki/Kalman_filter 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. |
<center> | <center> | ||
Line 8: | Line 8: | ||
| SimpleMatrix || 1875 | | SimpleMatrix || 1875 | ||
|- | |- | ||
− | | | + | | Operations || 1280 |
|- | |- | ||
| Equations || 1698 | | Equations || 1698 | ||
Line 16: | Line 16: | ||
__TOC__ | __TOC__ | ||
− | + | ||
− | * [https://github.com/lessthanoptimal/ejml/blob/ | + | External Resources: |
− | * [https://github.com/lessthanoptimal/ejml/blob/ | + | * [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterSimple.java KalmanFilterSimple] |
− | * [https://github.com/lessthanoptimal/ejml/blob/ | + | * [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterOperations.java KalmanFilterOperations] |
+ | * [https://github.com/lessthanoptimal/ejml/blob/v0.31/examples/src/org/ejml/example/KalmanFilterEquation.java KalmanFilterEquation] | ||
+ | * <disqus>Discuss this example</disqus> | ||
---- | ---- | ||
Line 32: | Line 34: | ||
* read and write, but the performance is degraded due to excessive creation/destruction of | * 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 | * memory and the use of more generic algorithms. This also demonstrates how code can be | ||
− | * seamlessly implemented using both SimpleMatrix and | + | * seamlessly implemented using both SimpleMatrix and DMatrixRMaj. This allows code |
* to be quickly prototyped or to be written either by novices or experts. | * to be quickly prototyped or to be written either by novices or experts. | ||
* | * | ||
Line 40: | Line 42: | ||
// kinematics description | // kinematics description | ||
− | private SimpleMatrix F | + | private SimpleMatrix F,Q,H; |
− | |||
− | |||
// sytem state estimate | // sytem state estimate | ||
− | private SimpleMatrix x | + | private SimpleMatrix x,P; |
− | |||
− | |||
@Override | @Override | ||
− | public void configure( | + | 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 57: | Line 55: | ||
@Override | @Override | ||
− | public void setState( | + | 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); | ||
Line 72: | Line 70: | ||
@Override | @Override | ||
− | public void update( | + | 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 94: | Line 92: | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getState() { |
return x.getMatrix(); | return x.getMatrix(); | ||
} | } | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getCovariance() { |
return P.getMatrix(); | return P.getMatrix(); | ||
} | } | ||
Line 105: | Line 103: | ||
</syntaxhighlight> | </syntaxhighlight> | ||
− | == | + | == Operations Example == |
<syntaxhighlight lang="java"> | <syntaxhighlight lang="java"> | ||
/** | /** | ||
* A Kalman filter that is implemented using the operations API, which is procedural. Much of the excessive | * 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 | + | * memory creation/destruction has been reduced from the KalmanFilterSimple. A specialized solver is |
− | * under to invert the SPD matrix. | + | * under to invert the SPD matrix. |
− | * | + | * |
* @author Peter Abeles | * @author Peter Abeles | ||
*/ | */ | ||
Line 118: | Line 116: | ||
// kinematics description | // kinematics description | ||
− | private | + | private DMatrixRMaj F,Q,H; |
− | |||
− | |||
// system state estimate | // system state estimate | ||
− | private | + | private DMatrixRMaj x,P; |
− | |||
// these are predeclared for efficiency reasons | // these are predeclared for efficiency reasons | ||
− | private | + | private DMatrixRMaj a,b; |
− | private | + | private DMatrixRMaj y,S,S_inv,c,d; |
− | private | + | private DMatrixRMaj K; |
− | private LinearSolver< | + | private LinearSolver<DMatrixRMaj> solver; |
@Override | @Override | ||
− | public void configure( | + | public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) { |
this.F = F; | this.F = F; | ||
this.Q = Q; | this.Q = Q; | ||
Line 142: | Line 137: | ||
int dimenZ = H.numRows; | int dimenZ = H.numRows; | ||
− | a = new | + | a = new DMatrixRMaj(dimenX,1); |
− | b = new | + | b = new DMatrixRMaj(dimenX,dimenX); |
− | y = new | + | y = new DMatrixRMaj(dimenZ,1); |
− | S = new | + | S = new DMatrixRMaj(dimenZ,dimenZ); |
− | S_inv = new | + | S_inv = new DMatrixRMaj(dimenZ,dimenZ); |
− | c = new | + | c = new DMatrixRMaj(dimenZ,dimenX); |
− | d = new | + | d = new DMatrixRMaj(dimenX,dimenZ); |
− | K = new | + | K = new DMatrixRMaj(dimenX,dimenZ); |
− | x = new | + | x = new DMatrixRMaj(dimenX,1); |
− | P = new | + | P = new DMatrixRMaj(dimenX,dimenX); |
// covariance matrices are symmetric positive semi-definite | // covariance matrices are symmetric positive semi-definite | ||
− | solver = | + | solver = LinearSolverFactory_DDRM.symmPosDef(dimenX); |
} | } | ||
@Override | @Override | ||
− | public void setState( | + | public void setState(DMatrixRMaj x, DMatrixRMaj P) { |
this.x.set(x); | this.x.set(x); | ||
this.P.set(P); | this.P.set(P); | ||
Line 178: | Line 173: | ||
@Override | @Override | ||
− | public void update( | + | public void update(DMatrixRMaj z, DMatrixRMaj R) { |
// y = z - H x | // y = z - H x | ||
mult(H,x,y); | mult(H,x,y); | ||
Line 205: | Line 200: | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getState() { |
return x; | return x; | ||
} | } | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getCovariance() { |
return P; | return P; | ||
} | } | ||
Line 227: | Line 222: | ||
// system state estimate | // system state estimate | ||
− | private | + | private DMatrixRMaj x,P; |
− | |||
private Equation eq; | private Equation eq; | ||
Line 237: | Line 231: | ||
@Override | @Override | ||
− | public void configure( | + | public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H) { |
int dimenX = F.numCols; | int dimenX = F.numCols; | ||
− | x = new | + | x = new DMatrixRMaj(dimenX,1); |
− | P = new | + | P = new DMatrixRMaj(dimenX,dimenX); |
eq = new Equation(); | eq = new Equation(); | ||
Line 250: | Line 244: | ||
// 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 | + | eq.alias(new DMatrixRMaj(1,1),"z"); |
− | eq.alias(new | + | 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 265: | Line 259: | ||
@Override | @Override | ||
− | public void setState( | + | public void setState(DMatrixRMaj x, DMatrixRMaj P) { |
this.x.set(x); | this.x.set(x); | ||
this.P.set(P); | this.P.set(P); | ||
Line 277: | Line 271: | ||
@Override | @Override | ||
− | public void update( | + | 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 | ||
Line 289: | Line 283: | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getState() { |
return x; | return x; | ||
} | } | ||
@Override | @Override | ||
− | public | + | public DMatrixRMaj getCovariance() { |
return P; | return P; | ||
} | } | ||
} | } | ||
</syntaxhighlight> | </syntaxhighlight> |
Revision as of 12:37, 18 May 2017
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:
- KalmanFilterSimple
- KalmanFilterOperations
- KalmanFilterEquation
- <disqus>Discuss this example</disqus>
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 LinearSolver<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"); eq.alias(R,"R");
updateY.perform();
updateK.perform();
updateX.perform();
updateP.perform();
}
@Override
public DMatrixRMaj getState() {
return x;
}
@Override
public DMatrixRMaj getCovariance() {
return P;
}
}