// -----------------------------------------
// 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());
}
int main() {
A a(42);
std::cout << "proof: " << a.*get(A_f()) << std::endl;
}
