List<KalmanFilter> all = new ArrayList<KalmanFilter>();
all.add( new KalmanFilterOps() );
all.add( new KalmanFilterAlg() );
all.add( simple );
DenseMatrix64F priorX = new DenseMatrix64F(9,1, true, 0.5, -0.2, 0, 0, 0.2, -0.9, 0, 0.2, -0.5);
DenseMatrix64F priorP = CommonOps.identity(9);
DenseMatrix64F F = BenchmarkKalmanPerformance.createF(T);
DenseMatrix64F Q = BenchmarkKalmanPerformance.createQ(T,0.1);
DenseMatrix64F H = BenchmarkKalmanPerformance.createH();
for( KalmanFilter f : all ) {
f.configure(F,Q,H);
f.setState(priorX,priorP);
f.predict();
}
for( KalmanFilter f : all ) {
compareFilters(simple,f);
}
DenseMatrix64F z = new DenseMatrix64F(H.numRows,1);
DenseMatrix64F R = CommonOps.identity(H.numRows);
for( KalmanFilter f : all ) {
f.update(z,R);
}