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