// -----------------------------------------
// tests
// -----------------------------------------
TEST(ExtendedKalmanFilterTest, initialization)
{
ExtendedKalmanFilter ekf;
// check setting required params
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "STM missing"
ekf.setStateTransitionModel(f);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "Jacobian of STM missing"
ekf.setJacobianOfStateTransitionModel(df);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "OM missing"
ekf.setObservationModel(h);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "Jacobian of OM missing"
ekf.setJacobianOfObservationModel(dh);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "PNC missing"
MatrixXd Q(1,1); Q << 0.1;
ekf.setProcessNoiseCovariance(Q);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "MNC missing"
MatrixXd R(1,1); R << 10;
ekf.setMeasurementNoiseCovariance(R);
EXPECT_NO_THROW(ekf.validate()); // all required params given
// check setting optional params
// optional params doesn't set new sizes
MatrixXd P0_f(2,2); P0_f << 1, 0, 0, 1;
ekf.setInitialErrorCovariance(P0_f);
EXPECT_NO_THROW(ekf.validate());
EXPECT_EQ(ekf.getState().size(), 1);
VectorXd x_f(2); x_f << 0,1;
ekf.setInitialState(x_f);
EXPECT_NO_THROW(ekf.validate());
EXPECT_NE(ekf.getState().size(), 2);
// required param Q sets new size of x and out
MatrixXd Q2(2,2); Q2 << 0.1, 0, 0, 0.1;
ekf.setProcessNoiseCovariance(Q2);
EXPECT_NO_THROW(ekf.validate());
EXPECT_EQ(ekf.getState().size(), 2);
EXPECT_EQ(ekf.getLastEstimate().size(), 2);
// check initialization of output
Output out = ekf.getLastEstimate();
OutputValue defaultOutVal;
EXPECT_GT(out.size(), 0);
EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());
// check setting the control input
InputValue ctrl(0);
Input in_ctrl(ctrl);
EXPECT_THROW(ekf.setControlInput(in_ctrl), length_error);
ekf.setControlInputSize(1);
EXPECT_NO_THROW(ekf.setControlInput(in_ctrl));
}
/*
* todo : Need to fix the problem
*/
void Reconstruct:: SecondOrderMUSCL(Data::DataStorage& Wcll, Data::DataStorage& Wcl, Data::DataStorage& Wcr, Data::DataStorage& Wcrr,
vector<double>& xcll, vector<double>& xcl, vector<double>& xcr, vector<double>& xcrr,
vector<double>& xf, bool use_limiter, int limiter,
Data::DataStorage& Wp, Data::DataStorage& Wm)
{
double dx, dx_plus, dx_minus, dx_f, dx_fp;
Math::VECTOR x(xcl, xcr);
Math::VECTOR x_plus(xcr, xcrr);
Math::VECTOR x_minus(xcll, xcl);
Math::VECTOR x_f(xcl, xf);
dx = x.length();
dx_plus = x_plus.length();
dx_minus = x_minus.length();
dx_f = x_f.length();
dx_fp = dx - dx_f;
double dW;
double dW_plus;
double dW_minus;
double r, alpha;
double phi;
double W_min;
double W_max;
bool error = false;
for (int index = 0; index <= Wcll.numData-1; index++)
{
dW = (Wcr(index) - Wcl(index));
if (dW != 0.0 && dx > 1.0e-10)
{
dW = dW / dx;
dW_minus = Wcl(index) - Wcll(index);
dW_plus = Wcrr(index) - Wcr(index);
if (dx_minus > 1.0e-10 && dW_minus != 0.0)
{
dW_minus /= dx_minus;
r = dW / dW_minus;
alpha = dx / dx_f;
phi = Limiter(r, alpha, limiter);
}
else
{
phi = 0.0;
}
Wp(index) = Wcl(index) + phi*dW*dx_f;
if (dx_plus > 1.0e-10 && dW_plus != 0.0)
{
dW_plus /= dx_plus;
r = dW_plus / dW;
alpha = dx / dx_fp;
phi = Limiter(r, alpha, limiter);
}
else
{
phi = 0.0;
}
Wm(index) = Wcr(index) - phi*dW*dx_fp;
//if (Wp(index) != Wp(index)) throw Common::ExceptionNaNValue (FromHere(), "NaN value in ReconstructMUSCL");
//if (Wp(index) == numeric_limits<double>::infinity()) throw Common::ExceptionInfiniteValue (FromHere(), "Infinite value in ReconstructMUSCL");
//if (Wm(index) != Wm(index)) throw Common::ExceptionNaNValue (FromHere(), "NaN value in ReconstructMUSCL");
//if (Wm(index) == numeric_limits<double>::infinity()) throw Common::ExceptionInfiniteValue (FromHere(), "Infinite value in ReconstructMUSCL");
W_min = Math::fmin<double>(Wcl(index), Wcr(index));
W_max = Math::fmax<double>(Wcl(index), Wcr(index));
if (Wp(index) < W_min || Wm(index) < W_min || Wp(index) > W_max ||Wm(index) > W_max)
{
error = true;
break;
}
/*
if (Wp(index) < W_min) Wp(index) = W_min;
if (Wm(index) < W_min) Wm(index) = W_min;
if (Wp(index) > W_max) Wp(index) = W_max;
if (Wm(index) > W_max) Wm(index) = W_max;
*/
}
else
{
Wp(index) = Wcl(index);
Wm(index) = Wcr(index);
}
}
if (error == true)
{
Wp = Wcl;
Wm = Wcr;
}
}
void Reconstruct::SecondOrderMUSCL_ver2(Data::DataStorage& Wcll, Data::DataStorage& Wcl, Data::DataStorage& Wcr, Data::DataStorage& Wcrr,
vector<double>& xcll, vector<double>& xcl, vector<double>& xcr, vector<double>& xcrr,
vector<double>& xf, bool use_limiter, int limiter,
Data::DataStorage& Wp, Data::DataStorage& Wm)
{
double dx, dx_plus, dx_minus, dx_f, dx_fp;
Math::VECTOR x(xcl, xcr);
Math::VECTOR x_plus(xcr, xcrr);
Math::VECTOR x_minus(xcll, xcl);
Math::VECTOR x_f(xcl, xf);
dx = x.length();
dx_plus = x_plus.length();
dx_minus = x_minus.length();
dx_f = x_f.length();
dx_fp = dx - dx_f;
double dxf_dx = dx_f / dx;
double dxfp_dx = dx_fp / dx;
double dxp_dx = dx_plus / dx;
double dxm_dx = dx_minus / dx;
double dW;
double dW_plus;
double dW_minus;
double r, alpha;
double phi;
double W_min;
double W_max;
bool error = false;
for (int index = 0; index <= Wcll.numData-1; index++)
{
dW = (Wcr(index) - Wcl(index));
if (dW != 0.0 && dx > 1.0e-10)
{
dW_minus = Wcl(index) - Wcll(index);
dW_plus = Wcrr(index) - Wcr(index);
if (dW_plus != 0.0) r = dW / dW_plus * dxp_dx;
else r = 0.0;
phi = Limiter2(r, limiter);
Wp(index) = Wcl(index) + phi*dW*dxf_dx;
if (dW_minus != 0.0) r = dW / dW_minus * dxm_dx;
else r = 0.0;
phi = Limiter2(r, limiter);
Wm(index) = Wcr(index) - phi*dW*dxfp_dx;
W_min = Math::fmin<double>(Wcl(index), Wcr(index));
W_max = Math::fmax<double>(Wcl(index), Wcr(index));
if (Wp(index) < W_min || Wm(index) < W_min || Wp(index) > W_max ||Wm(index) > W_max)
{
error = true;
break;
}
}
else
{
Wp(index) = Wcl(index);
Wm(index) = Wcr(index);
}
}
if (error == true)
{
Wp = Wcl;
Wm = Wcr;
}
}
