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);
  }