TEST(ExtendedKalmanFilterTest, validation)
{
ExtendedKalmanFilter ekf;
// check validation itself (e.g. invalid sizes of vectors/matrices,
// empty callbacks, etc.)
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "STM missing"
ekf.setStateTransitionModel(0);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "STM missing"
ekf.setStateTransitionModel(f);
ekf.setJacobianOfStateTransitionModel(0);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "STMJ missing"
ekf.setJacobianOfStateTransitionModel(df);
ekf.setObservationModel(0);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "OM missing"
ekf.setObservationModel(h);
ekf.setJacobianOfObservationModel(0);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "OMJ missing"
ekf.setJacobianOfObservationModel(dh);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "PNC missing"
MatrixXd Q_f(2,1); Q_f << 0.1, 1;
ekf.setProcessNoiseCovariance(Q_f);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "Q not square"
MatrixXd Q1(1,1); Q1 << 0.1;
ekf.setProcessNoiseCovariance(Q1);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "MNC missing"
MatrixXd R_f(2,1); R_f << 10, 1;
ekf.setMeasurementNoiseCovariance(R_f);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "R not square"
MatrixXd R(1,1); R << 10;
ekf.setMeasurementNoiseCovariance(R);
EXPECT_NO_THROW(ekf.validate()); // all required params given
// optional params
VectorXd x(2); x << 0, 1;
ekf.setInitialState(x);
EXPECT_NO_THROW(ekf.validate());
EXPECT_NE(ekf.getState().size(), 2);
MatrixXd Q(2,2); Q << 0.1, 0, 0, 0.1;
ekf.setProcessNoiseCovariance(Q);
EXPECT_NO_THROW(ekf.validate());
ekf.setInitialState(x);
EXPECT_NO_THROW(ekf.validate());
EXPECT_EQ(ekf.getState().size(), 2); // sizes now ok
// size of R cannot be checked, must match number of formulas in h
// -> check during compile time
}
// -----------------------------------------
// tests
// -----------------------------------------
TEST(KalmanFilterTest, initializationAndValidation)
{
KalmanFilter kf;
// check setting required params
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "STM missing"
MatrixXd A_f(2,1); A_f << 1, 0;
kf.setStateTransitionModel(A_f);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "OM missing"
MatrixXd H(1,2); H << 1,0;
kf.setObservationModel(H);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "PNC missing"
MatrixXd Q(2,2); Q << 0.1,0,0,0.1;
kf.setProcessNoiseCovariance(Q);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "MNC missing"
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "STM invalid size"
MatrixXd A(2,2); A << 1,0,0,1;
kf.setStateTransitionModel(A);
EXPECT_NO_THROW(kf.validate()); // all required params given
MatrixXd R_f(2,1); R_f << 10,1;
kf.setMeasurementNoiseCovariance(R_f);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "MNC invalid size"
kf.setMeasurementNoiseCovariance(R);
MatrixXd H_f(3,1); H_f << 1,0,2;
kf.setObservationModel(H_f);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "OM invalid size"
kf.setObservationModel(H);
// check setting optional params
MatrixXd B_f(1,1); B_f << 0;
kf.setControlInputModel(B_f);
EXPECT_THROW(kf.validate(), IEstimator::estimator_error); // "CIM invalid size"
MatrixXd B(2,1); B << 0,0;
kf.setControlInputModel(B);
EXPECT_NO_THROW(kf.validate());
VectorXd x(2); x << 0,1;
kf.setInitialState(x);
EXPECT_NO_THROW(kf.validate());
// check initialization of output
Output out = kf.getLastEstimate();
OutputValue defaultOutVal;
EXPECT_GT(out.size(), 0);
EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());
}
//<F> -> - <F> | <R>
node_t* F_f() {
node_t *p = getNode("<F>");
if(tk.name == "-") { // -<F>
p->child1 = getNode("-");
tk = scanner(fp, lineNum);
p->child2 = F_f();
return p;
} else if((tk.name == "(") || tk.tokenId == IDENT_tk || tk.tokenId == NUM_tk) { // <R>
p->child1 = R_f();
return p;
} else {
error("Got token " + tk.name + " expected (, ID, NUM, or - token\n");
}
return p;
}
