TEST(KalmanFilterTest, validationEffect)
{
KalmanFilter kf;
MatrixXd A(1,1); A << 1;
kf.setStateTransitionModel(A);
MatrixXd H(1,1); H << 1;
kf.setObservationModel(H);
MatrixXd Q(1,1); Q << 0.1;
kf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
// validate has an effect?
InputValue measurement(1);
Input in(measurement);
Output out;
EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error); // "not yet validated"
EXPECT_NO_THROW(kf.validate());
// changing a parameter -> KF must be re-validated
kf.setProcessNoiseCovariance(Q);
EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error); // "not yet validated"
EXPECT_NO_THROW(kf.validate());
// KF should now be released and return an estimate (should not be
// the default)
EXPECT_NO_THROW(out = kf.estimate(in));
EXPECT_GT(out.size(), 0);
EXPECT_EQ(kf.getState().size(), out.size());
}
void TargetTracker::tracking(std::vector<TargetState> states)
{
std::vector<TargetState>::const_iterator iter;
TargetState tState;
KalmanFilter *filter;
SingleTarget *target;
qDebug() << "测量值：";
for (iter = states.begin(); iter != states.end(); ++iter)
{
tState = *iter;
tState.state.print();
filter = findFilter(tState.groupId, tState.targetId);
target = findTarget(tState.groupId, tState.targetId);
if (target == NULL) continue;
if (filter == NULL) continue;
if (target->getStateCount() == 0) {
// 第一次获取目标的状态，直接保存，不做任何计算。
target->addState(tState.state);
filter->setState(tState.state.getData());
continue;
}
//计算一步预测值
Matrix matrix = filter->estimate();
// 根据测量值更新预测数据
filter->updateByMeasure(tState.state.getData());
State s;
s.setData(matrix);
s.setTime(tState.time);
target->addState(s);
}
//项目验收后去掉
initMessage();
printTargetGroups();
}
TEST(KalmanFilterTest, functionality)
{
KalmanFilter kf;
MatrixXd A(1,1); A << 1;
kf.setStateTransitionModel(A);
MatrixXd H(1,1); H << 1;
kf.setObservationModel(H);
MatrixXd Q(1,1); Q << 0.1;
kf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
kf.validate();
// check if the calculation of an estimate is correct
InputValue measurement(1);
Input in(measurement);
Output out = kf.estimate(in);
EXPECT_EQ(out.size(), 1);
EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);
in[0].setValue(5);
EXPECT_NO_THROW(out = kf.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);
// missing measurement
InputValue missingValue;
Input inMissing(missingValue);
EXPECT_NO_THROW(out = kf.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);
// another example ---------------------------------
KalmanFilter kf2;
MatrixXd A2(2,2); A2 << 1,0.1,0,1;
kf2.setStateTransitionModel(A2);
MatrixXd H2(1,2); H2 << 0,1;
kf2.setObservationModel(H2);
MatrixXd Q2(2,2); Q2 << 0.1,0,0,0.1;
kf2.setProcessNoiseCovariance(Q2);
MatrixXd R2(1,1); R2 << 10;
kf2.setMeasurementNoiseCovariance(R2);
kf2.validate();
in[0].setValue(1);
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_EQ(out.size(), 2);
EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);
// add control input
MatrixXd B2(2,1); B2 << 0,1;
kf2.setControlInputModel(B2); // needs to be validated
InputValue ctrl(0.5);
Input in_ctrl(ctrl);
kf2.setControlInput(in_ctrl);
in[0].setValue(5);
EXPECT_NO_THROW(kf2.validate());
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.0053, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.20098, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.1951, 0.0001);
// pass control input with invalid size
in_ctrl.add(ctrl); // -> u.size = 2
// setting invalid control input throws exception
EXPECT_THROW(kf2.setControlInput(in_ctrl), length_error);
// changing control input model works
MatrixXd B3(2,2); B3 << 0,0,0,0;
EXPECT_NO_THROW(kf2.setControlInputModel(B3));
EXPECT_ANY_THROW(out = kf2.estimate(in)); // not validated
EXPECT_NO_THROW(kf2.validate());
// missing measurement
EXPECT_NO_THROW(out = kf2.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.0651, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.3048, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.2867, 0.0001);
}
