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