private void compareFilters(KalmanFilter a, KalmanFilter b)
{
DMatrixRMaj testX = b.getState();
DMatrixRMaj testP = b.getCovariance();
DMatrixRMaj X = a.getState();
DMatrixRMaj P = a.getCovariance();
EjmlUnitTests.assertEquals(testX, X, UtilEjml.TEST_F64);
EjmlUnitTests.assertEquals(testP, P, UtilEjml.TEST_F64);
}
public void testNumericalJacobian()
{
JacobianTestFunction func = new JacobianTestFunction();
DMatrixRMaj param = new DMatrixRMaj(3, 1, true, 2, -1, 4);
LevenbergMarquardt alg = new LevenbergMarquardt(func);
DMatrixRMaj X = RandomMatrices_DDRM.rectangle(NUM_PTS, 1, rand);
DMatrixRMaj numJacobian = new DMatrixRMaj(3, NUM_PTS);
DMatrixRMaj analyticalJacobian = new DMatrixRMaj(3, NUM_PTS);
alg.configure(param, X, new DMatrixRMaj(NUM_PTS, 1));
alg.computeNumericalJacobian(param, X, numJacobian);
func.deriv(X, analyticalJacobian);
EjmlUnitTests.assertEquals(analyticalJacobian, numJacobian, 1e-6);
}
/**
* Runs the simple optimization problem with a set of randomly generated inputs.
*
* @param numPoints How many sample points there are.
*/
public void runTrivial(int numPoints)
{
JacobianTestFunction func = new JacobianTestFunction();
DMatrixRMaj paramInit = new DMatrixRMaj(3, 1);
DMatrixRMaj param = new DMatrixRMaj(3, 1, true, 2, -1, 4);
LevenbergMarquardt alg = new LevenbergMarquardt(func);
DMatrixRMaj X = RandomMatrices_DDRM.rectangle(numPoints, 1, rand);
DMatrixRMaj Y = new DMatrixRMaj(numPoints, 1);
func.compute(param, X, Y);
alg.optimize(paramInit, X, Y);
DMatrixRMaj foundParam = alg.getParameters();
Assert.IsTrue(Math.Abs(0 - alg.getFinalCost()) < UtilEjml.TEST_F64);
EjmlUnitTests.assertEquals(param, foundParam, 1e-6);
}