Example #1
0
void FGStateSpace::numericalJacobian(std::vector< std::vector<double> >  & J, ComponentVector & y,
                                     ComponentVector & x, const std::vector<double> & y0, const std::vector<double> & x0, double h, bool computeYDerivative)
{
    int nX = x.getSize();
    int nY = y.getSize();
    double f1 = 0, f2 = 0, fn1 = 0, fn2 = 0;
    J.resize(nY);
    for (int iY=0;iY<nY;iY++)
    {
        J[iY].resize(nX);
        for (int iX=0;iX<nX;iX++)
        {
            x.set(x0);
            x.set(iX,x.get(iX)+h);
            if (computeYDerivative) f1 = y.getDeriv(iY);
            else f1 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)+2*h);
            if (computeYDerivative) f2 = y.getDeriv(iY);
            else f2 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)-h);
            if (computeYDerivative) fn1 = y.getDeriv(iY);
            else fn1 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)-2*h);
            if (computeYDerivative) fn2 = y.getDeriv(iY);
            else fn2 = y.get(iY);

            J[iY][iX] = (8*(f1-fn1)-(f2-fn2))/(12*h); // 3rd order taylor approx from lewis, pg 203
            x.set(x0);

            if (m_fdm->GetDebugLevel() > 1)
            {
                std::cout << std::scientific << "\ty:\t" << y.getName(iY) << "\tx:\t"
                          << x.getName(iX)
                          << "\tfn2:\t" << fn2 << "\tfn1:\t" << fn1
                          << "\tf1:\t" << f1 << "\tf2:\t" << f2
                          << "\tf1-fn1:\t" << f1-fn1
                          << "\tf2-fn2:\t" << f2-fn2
                          << "\tdf/dx:\t" << J[iY][iX]
                          << std::fixed << std::endl;
            }
        }
    }
}
Example #2
0
void FGStateSpace::numericalJacobian(std::vector< std::vector<double> >  & J, ComponentVector & y,
                                     ComponentVector & x, const std::vector<double> & y0, const std::vector<double> & x0, double h, bool computeYDerivative)
{
    size_t nX = x.getSize();
    size_t nY = y.getSize();
    double f1 = 0, f2 = 0, fn1 = 0, fn2 = 0;
    J.resize(nY);
    for (unsigned int iY=0;iY<nY;iY++)
    {
        J[iY].resize(nX);
        for (unsigned int iX=0;iX<nX;iX++)
        {
            x.set(x0);
            x.set(iX,x.get(iX)+h);
            if (computeYDerivative) f1 = y.getDeriv(iY);
            else f1 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)+2*h);
            if (computeYDerivative) f2 = y.getDeriv(iY);
            else f2 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)-h);
            if (computeYDerivative) fn1 = y.getDeriv(iY);
            else fn1 = y.get(iY);

            x.set(x0);
            x.set(iX,x.get(iX)-2*h);
            if (computeYDerivative) fn2 = y.getDeriv(iY);
            else fn2 = y.get(iY);

			double diff1 = f1-fn1;
			double diff2 = f2-fn2;

			// correct for angle wrap
			if (x.getComp(iX)->getUnit().compare("rad") == 0) {
				while(diff1 > M_PI) diff1 -= 2*M_PI;
				if(diff1 < -M_PI) diff1 += 2*M_PI;
				if(diff2 > M_PI) diff2 -= 2*M_PI;
				if(diff2 < -M_PI) diff2 += 2*M_PI;
			} else if (x.getComp(iX)->getUnit().compare("deg") == 0) {
				if(diff1 > 180) diff1 -= 360;
				if(diff1 < -180) diff1 += 360;
				if(diff2 > 180) diff2 -= 360;
				if(diff2 < -180) diff2 += 360;
			}
            J[iY][iX] = (8*diff1-diff2)/(12*h); // 3rd order taylor approx from lewis, pg 203

            x.set(x0);

            if (m_fdm->GetDebugLevel() > 1)
            {
                std::cout << std::scientific << "\ty:\t" << y.getName(iY) << "\tx:\t"
                          << x.getName(iX)
                          << "\tfn2:\t" << fn2 << "\tfn1:\t" << fn1
                          << "\tf1:\t" << f1 << "\tf2:\t" << f2
                          << "\tf1-fn1:\t" << f1-fn1
                          << "\tf2-fn2:\t" << f2-fn2
                          << "\tdf/dx:\t" << J[iY][iX]
                          << std::fixed << std::endl;
            }
        }
    }
}