fly/flightgear
1
0
Fork 0
Code Activity

Removed obsolete files from JSBSim.

Browse source 
These files were originally included for special trim routines. But these routines are no longer maintained in JSBSim and have already been partially removed from FlightGear.
This commit is contained in:
Bertrand Coconnier 2018-01-13 19:46:41 +01:00
parent 08a877cb8e
commit c9e276e5dd
5 changed files with 0 additions and 1605 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/FDM/JSBSim/CMakeLists.txt
View file

@ -40,8 +40,6 @@ set(HEADERS
math/FGRealValue.h
math/FGRungeKutta.h
math/FGTable.h
math/FGNelderMead.h
math/FGStateSpace.h
models/FGAccelerations.h
models/FGAerodynamics.h
models/FGAircraft.h
@ -134,8 +132,6 @@ set(SOURCES
math/FGRealValue.cpp
math/FGRungeKutta.cpp
math/FGTable.cpp
math/FGNelderMead.cpp
math/FGStateSpace.cpp
models/FGAccelerations.cpp
models/FGAerodynamics.cpp
models/FGAircraft.cpp

380
src/FDM/JSBSim/math/FGNelderMead.cpp
View file

@ -1,380 +0,0 @@
/*
* FGNelderMead.cpp
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGNelderMead.cpp is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGNelderMead.cpp is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "FGNelderMead.h"
#include <limits>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <ctime>
namespace JSBSim
{
FGNelderMead::FGNelderMead(Function * f, const std::vector<double> & initialGuess,
const std::vector<double> & lowerBound,
const std::vector<double> & upperBound,
const std::vector<double> & initialStepSize, int iterMax,
double rtol, double abstol, double speed, double randomization,
bool showConvergeStatus,
bool showSimplex, bool pause, Callback * callback) :
m_f(f), m_callback(callback), m_randomization(randomization),
m_lowerBound(lowerBound), m_upperBound(upperBound),
m_nDim(initialGuess.size()), m_nVert(m_nDim+1),
m_iMax(1), m_iNextMax(1), m_iMin(1),
m_simplex(m_nVert), m_cost(m_nVert), m_elemSum(m_nDim),
m_status(1),
initialGuess(initialGuess), initialStepSize(initialStepSize),
iterMax(iterMax), iter(), rtol(rtol), abstol(abstol),
speed(speed), showConvergeStatus(showConvergeStatus), showSimplex(showSimplex),
pause(pause), rtolI(), minCostPrevResize(1), minCost(), minCostPrev(), maxCost(),
nextMaxCost()
{
srand ( time(NULL) ); // seed random number generator
}
void FGNelderMead::update()
{
std::cout.precision(3);
// reinitialize simplex whenever rtol condition is met
if ( rtolI < rtol || iter == 0)
{
std::vector<double> guess(m_nDim);
if (iter == 0)
{
//std::cout << "constructing simplex" << std::endl;
guess = initialGuess;
}
else
{
if (std::abs(minCost-minCostPrevResize) < std::numeric_limits<float>::epsilon())
{
m_status = -1;
throw std::runtime_error("unable to escape local minimum!");
}
//std::cout << "reinitializing step size" << std::endl;
guess = m_simplex[m_iMin];
minCostPrevResize = minCost;
}
constructSimplex(guess,initialStepSize);
}
// find vertex costs
for (unsigned int vertex=0;vertex<m_nVert;vertex++)
{
try
{
m_cost[vertex] = eval(m_simplex[vertex]);
}
catch (...)
{
m_status = -1;
throw;
}
}
// find max cost, next max cost, and min cost
m_iMax = m_iNextMax = m_iMin = 0;
for (unsigned int vertex=0;vertex<m_nVert;vertex++)
{
if ( m_cost[vertex] > m_cost[m_iMax] )
{
m_iMax = vertex;
}
else if ( m_cost[vertex] > m_cost[m_iNextMax] || m_iMax == m_iNextMax ) m_iNextMax = vertex;
else if ( m_cost[vertex] < m_cost[m_iMin] ) m_iMin = vertex;
}
// callback
if (m_callback) m_callback->eval(m_simplex[m_iMin]);
// compute relative tolerance
rtolI = 2*std::abs(m_cost[m_iMax] -
m_cost[m_iMin])/(std::abs(m_cost[m_iMax]+std::abs(m_cost[m_iMin])+
std::numeric_limits<double>::epsilon()));
// check for max iteration break condition
if (iter > iterMax)
{
m_status = -1;
throw std::runtime_error("max iterations exceeded!");
}
// check for convergence break condition
else if ( m_cost[m_iMin] < abstol )
{
//std::cout << "\nsimplex converged" << std::endl;
m_status = 0;
return;
}
// compute element sum of simplex vertices
for (unsigned int dim=0;dim<m_nDim;dim++)
{
m_elemSum[dim] = 0;
for (unsigned int vertex=0;vertex<m_nVert;vertex++)
m_elemSum[dim] += m_simplex[vertex][dim];
}
// min and max costs
minCostPrev = minCost;
minCost = m_cost[m_iMin];
maxCost = m_cost[m_iMax];
nextMaxCost = m_cost[m_iNextMax];
// output cost and simplex
if (showConvergeStatus)
{
if ( (minCostPrev + std::numeric_limits<float>::epsilon() )
< minCost && minCostPrev != 0)
{
std::cout << "\twarning: simplex cost increased"
<< std::scientific
<< "\n\tcost: " << minCost
<< "\n\tcost previous: " << minCostPrev
<< std::fixed << std::endl;
}
std::cout << "i: " << iter
<< std::scientific
<< "\tcost: " << m_cost[m_iMin]
<< "\trtol: " << rtolI
<< std::fixed
<< "\talpha: " << m_simplex[m_iMin][2]*180/M_PI
<< "\tbeta: " << m_simplex[m_iMin][5]*180/M_PI
<< "\tthrottle: " << m_simplex[m_iMin][0]
<< "\televator: " << m_simplex[m_iMin][1]
<< "\taileron: " << m_simplex[m_iMin][3]
<< "\trudder: " << m_simplex[m_iMin][4]
<< std::endl;
}
if (showSimplex)
{
std::cout << "simplex: " << std::endl;;
for (unsigned int j=0;j<m_nVert;j++)
std::cout << "\t" << std::scientific
<< std::setw(10) << m_cost[j];
std::cout << std::endl;
for (unsigned int j=0;j<m_nVert;j++) std::cout << "\t\t" << j;
std::cout << std::endl;
for (unsigned int i=0;i<m_nDim;i++)
{
for (unsigned int j=0;j<m_nVert;j++)
std::cout << "\t" << std::setw(10) << m_simplex[j][i];
std::cout << std::endl;
}
std::cout << std::fixed
<< "\n\tiMax: " << m_iMax
<< "\t\tiNextMax: " << m_iNextMax
<< "\t\tiMin: " << m_iMin << std::endl;
}
if (pause)
{
std::cout << "paused, press any key to continue" << std::endl;
std::cin.get();
}
// costs
try
{
// try inversion
double costTry = tryStretch(-1.0);
//std::cout << "cost Try 0: " << costTry << std::endl;
// if lower cost than best, then try further stretch by double speed factor
if (costTry < minCost)
{
double costTry0 = costTry;
costTry = tryStretch(speed);
//std::cout << "cost Try 1: " << costTry << std::endl;
if (showSimplex)
{
if (costTry < costTry0) std::cout << "inversion about: " << m_iMax << std::endl;
else std::cout << "inversion and stretch about: " << m_iMax << std::endl;
}
}
// otherwise try a contraction
else if (costTry > nextMaxCost)
{
// 1d contraction
costTry = tryStretch(1./speed);
//std::cout << "cost Try 2: " << costTry << std::endl;
// if greater than max cost, contract about min
if (costTry > maxCost)
{
if (showSimplex)
std::cout << "multiD contraction about: " << m_iMin << std::endl;
contract();
}
else
{
if (showSimplex)
std::cout << "contraction about: " << m_iMin << std::endl;
}
}
}
catch (...)
{
m_status = -1;
throw;
}
// iteration
iter++;
}
int FGNelderMead::status()
{
return m_status;
}
double FGNelderMead::getRandomFactor()
{
double randFact = 1+(float(rand() % 1000)/500-1)*m_randomization;
//std::cout << "random factor: " << randFact << std::endl;;
return randFact;
}
std::vector<double> FGNelderMead::getSolution()
{
return m_simplex[m_iMin];
}
double FGNelderMead::tryStretch(double factor)
{
// randomize factor so we can avoid locking situations
factor = factor*getRandomFactor();
// create trial vertex
double a= (1.0-factor)/m_nDim;
double b = a - factor;
std::vector<double> tryVertex(m_nDim);
for (unsigned int dim=0;dim<m_nDim;dim++)
{
tryVertex[dim] = m_elemSum[dim]*a - m_simplex[m_iMax][dim]*b;
boundVertex(tryVertex,m_lowerBound,m_upperBound);
}
// find trial cost
double costTry = eval(tryVertex);
// if trial cost lower than max
if (costTry < m_cost[m_iMax])
{
// update the element sum of the simplex
for (unsigned int dim=0;dim<m_nDim;dim++) m_elemSum[dim] +=
tryVertex[dim] - m_simplex[m_iMax][dim];
// replace the max vertex with the trial vertex
for (unsigned int dim=0;dim<m_nDim;dim++) m_simplex[m_iMax][dim] = tryVertex[dim];
// update the cost
m_cost[m_iMax] = costTry;
if (showSimplex) std::cout << "stretched\t" << m_iMax << "\tby : " << factor << std::endl;
}
return costTry;
}
void FGNelderMead::contract()
{
for (unsigned int dim=0;dim<m_nDim;dim++)
{
for (unsigned int vertex=0;vertex<m_nVert;vertex++)
{
m_simplex[vertex][dim] =
getRandomFactor()*0.5*(m_simplex[vertex][dim] +
m_simplex[m_iMin][dim]);
}
}
}
void FGNelderMead::constructSimplex(const std::vector<double> & guess,
const std::vector<double> & stepSize)
{
for (unsigned int vertex=0;vertex<m_nVert;vertex++)
{
m_simplex[vertex] = guess;
}
for (unsigned int dim=0;dim<m_nDim;dim++)
{
int vertex = dim + 1;
m_simplex[vertex][dim] += stepSize[dim]*getRandomFactor();
boundVertex(m_simplex[vertex],m_lowerBound,m_upperBound);
}
if (showSimplex)
{
std::cout << "simplex: " << std::endl;;
for (unsigned int j=0;j<m_nVert;j++) std::cout << "\t\t" << j;
std::cout << std::endl;
for (unsigned int i=0;i<m_nDim;i++)
{
for (unsigned int j=0;j<m_nVert;j++)
std::cout << "\t" << std::setw(10) << m_simplex[j][i];
std::cout << std::endl;
}
}
}
void FGNelderMead::boundVertex(std::vector<double> & vertex,
const std::vector<double> & lowerBound,
const std::vector<double> & upperBound)
{
for (unsigned int dim=0;dim<m_nDim;dim++)
{
if (vertex[dim] > upperBound[dim]) vertex[dim] = upperBound[dim];
else if (vertex[dim] < lowerBound[dim]) vertex[dim] = lowerBound[dim];
}
}
double FGNelderMead::eval(const std::vector<double> & vertex, bool check)
{
if (check) {
double cost0 = m_f->eval(vertex);
double cost1 = m_f->eval(vertex);
if ((cost0 - cost1) > std::numeric_limits<float>::epsilon()) {
std::stringstream msg;
msg.precision(10);
msg << std::scientific
<< "dynamics not stable!"
<< "\tdiff: " << cost1 - cost0
<< "\tcost0: " << cost0
<< "\tcost1: " << cost1
<< std::endl;
std::cout << msg.str();
//throw std::runtime_error(msg.str());
} else {
return cost1;
}
}
return m_f->eval(vertex);
}
} // JSBSim
// vim:ts=4:sw=4

97
src/FDM/JSBSim/math/FGNelderMead.h
View file

@ -1,97 +0,0 @@
/*
* FGNelderMead.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGNelderMead.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGNelderMead.h is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef JSBSim_FGNelderMead_H
#define JSBSim_FGNelderMead_H
#include <vector>
#include <limits>
#include <cstddef>
namespace JSBSim
{
class FGNelderMead
{
public:
class Function
{
public:
virtual double eval(const std::vector<double> & v) = 0;
virtual ~Function() {};
};
class Callback
{
public:
virtual void eval(const std::vector<double> & v) = 0;
virtual ~Callback() {};
};
FGNelderMead(Function * f, const std::vector<double> & initialGuess,
const std::vector<double> & lowerBound,
const std::vector<double> & upperBound,
const std::vector<double> & initialStepSize, int iterMax=2000,
double rtol=std::numeric_limits<float>::epsilon(),
double abstol=std::numeric_limits<float>::epsilon(),
double speed = 2.0,
double randomization=0.1,
bool showConvergeStatus=true,bool showSimplex=false,
bool pause=false,
Callback * callback=NULL);
std::vector<double> getSolution();
void update();
int status();
private:
// attributes
Function * m_f;
Callback * m_callback;
double m_randomization;
const std::vector<double> & m_lowerBound;
const std::vector<double> & m_upperBound;
size_t m_nDim, m_nVert;
int m_iMax, m_iNextMax, m_iMin;
std::vector< std::vector<double> > m_simplex;
std::vector<double> m_cost;
std::vector<double> m_elemSum;
int m_status;
const std::vector<double> & initialGuess;
const std::vector<double> & initialStepSize;
int iterMax, iter;
double rtol,abstol,speed;
bool showConvergeStatus, showSimplex, pause;
double rtolI, minCostPrevResize, minCost, minCostPrev,
maxCost, nextMaxCost;
// methods
double getRandomFactor();
double tryStretch(double factor);
void contract();
void constructSimplex(const std::vector<double> & guess, const std::vector<double> & stepSize);
void boundVertex(std::vector<double> & vertex,
const std::vector<double> & upperBound,
const std::vector<double> & lowerBound);
double eval(const std::vector<double> & vertex, bool check = false);
};
} // JSBSim
#endif
// vim:ts=4:sw=4

190
src/FDM/JSBSim/math/FGStateSpace.cpp
View file

@ -1,190 +0,0 @@
/*
* FGStateSpace.cpp
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGStateSpace.cpp is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGStateSpace.cpp is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "initialization/FGInitialCondition.h"
#include "FGStateSpace.h"
#include <limits>
#include <iomanip>
#include <string>
namespace JSBSim
{
void FGStateSpace::linearize(
std::vector<double> x0,
std::vector<double> u0,
std::vector<double> y0,
std::vector< std::vector<double> > & A,
std::vector< std::vector<double> > & B,
std::vector< std::vector<double> > & C,
std::vector< std::vector<double> > & D)
{
double h = 1e-4;
// A, d(x)/dx
numericalJacobian(A,x,x,x0,x0,h,true);
// B, d(x)/du
numericalJacobian(B,x,u,x0,u0,h,true);
// C, d(y)/dx
numericalJacobian(C,y,x,y0,x0,h);
// D, d(y)/du
numericalJacobian(D,y,u,y0,u0,h);
}
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;
}
}
}
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace::Component &c )
{
out << "\t" << c.getName()
<< "\t" << c.getUnit()
<< "\t:\t" << c.get();
return out;
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace::ComponentVector &v )
{
for (unsigned int i=0; i< v.getSize(); i++)
{
out << *(v.getComp(i)) << "\n";
}
out << "";
return out;
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace &ss )
{
out << "\nX:\n" << ss.x
<< "\nU:\n" << ss.u
<< "\nY:\n" << ss.y;
return out;
}
std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d )
{
std::streamsize width = out.width();
size_t nI = vec2d.size();
out << std::left << std::setw(1) << "[" << std::right;
for (unsigned int i=0;i<nI;i++)
{
//std::cout << "i: " << i << std::endl;
size_t nJ = vec2d[i].size();
for (unsigned int j=0;j<nJ;j++)
{
//std::cout << "j: " << j << std::endl;
if (i==0 && j==0) out << std::setw(width-1) << vec2d[i][j];
else out << std::setw(width) << vec2d[i][j];
if (j==nJ-1)
{
if ( i==nI-1 ) out << "]";
else out << ";\n";
}
else out << ",";
}
out << std::flush;
}
return out;
}
std::ostream &operator<<( std::ostream &out, const std::vector<double> &vec )
{
std::streamsize width = out.width();
size_t nI = vec.size();
out << std::left << std::setw(1) << "[" << std::right;
for (unsigned int i=0;i<nI;i++)
{
if (i==0) out << std::setw(width-1) << vec[i];
else out << std::setw(width) << vec[i];
if ( i==nI-1 ) out << "]";
else out << ";\n";
}
out << std::flush;
return out;
}
} // JSBSim
// vim:ts=4:sw=4

934
src/FDM/JSBSim/math/FGStateSpace.h
View file

@ -1,934 +0,0 @@
/*
* FGStateSpace.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGStateSpace.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGStateSpace.h is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef JSBSim_FGStateSpace_H
#define JSBSim_FGStateSpace_H
#include "FGFDMExec.h"
#include "models/FGPropulsion.h"
#include "models/FGAccelerations.h"
#include "models/propulsion/FGEngine.h"
#include "models/propulsion/FGThruster.h"
#include "models/propulsion/FGTurbine.h"
#include "models/propulsion/FGTurboProp.h"
#include "models/FGAuxiliary.h"
#include "models/FGFCS.h"
#include <fstream>
#include <iostream>
#include <limits>
namespace JSBSim
{
class FGStateSpace
{
public:
// component class
class Component
{
protected:
FGStateSpace * m_stateSpace;
FGFDMExec * m_fdm;
std::string m_name, m_unit;
public:
Component(const std::string & name, const std::string & unit) :
m_stateSpace(), m_fdm(), m_name(name), m_unit(unit) {};
virtual ~Component() {};
virtual double get() const = 0;
virtual void set(double val) = 0;
virtual double getDeriv() const
{
// by default should calculate using finite difference approx
std::vector<double> x0 = m_stateSpace->x.get();
double f0 = get();
double dt0 = m_fdm->GetDeltaT();
double time0 = m_fdm->GetSimTime();
m_fdm->Setdt(1./120.);
m_fdm->DisableOutput();
m_fdm->Run();
double f1 = get();
m_stateSpace->x.set(x0);
if (m_fdm->GetDebugLevel() > 1)
{
std::cout << std::scientific
<< "name: " << m_name
<< "\nf1: " << f0
<< "\nf2: " << f1
<< "\ndt: " << m_fdm->GetDeltaT()
<< "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT()
<< std::fixed << std::endl;
}
double deriv = (f1-f0)/m_fdm->GetDeltaT();
m_fdm->Setdt(dt0); // restore original value
m_fdm->Setsim_time(time0);
m_fdm->EnableOutput();
return deriv;
}
void setStateSpace(FGStateSpace * stateSpace)
{
m_stateSpace = stateSpace;
}
void setFdm(FGFDMExec * fdm)
{
m_fdm = fdm;
}
const std::string & getName() const
{
return m_name;
}
const std::string & getUnit() const
{
return m_unit;
}
};
// component vector class
class ComponentVector
{
public:
ComponentVector(FGFDMExec * fdm, FGStateSpace * stateSpace) :
m_stateSpace(stateSpace), m_fdm(fdm), m_components() {}
ComponentVector & operator=(ComponentVector & componentVector)
{
m_stateSpace = componentVector.m_stateSpace;
m_fdm = componentVector.m_fdm;
m_components = componentVector.m_components;
return *this;
}
ComponentVector(const ComponentVector & componentVector) :
m_stateSpace(componentVector.m_stateSpace),
m_fdm(componentVector.m_fdm),
m_components(componentVector.m_components)
{
}
void add(Component * comp)
{
comp->setStateSpace(m_stateSpace);
comp->setFdm(m_fdm);
m_components.push_back(comp);
}
size_t getSize() const
{
return m_components.size();
}
Component * getComp(int i) const
{
return m_components[i];
};
Component * getComp(int i)
{
return m_components[i];
};
double get(int i) const
{
return m_components[i]->get();
};
void set(int i, double val)
{
m_components[i]->set(val);
m_stateSpace->run();
};
double get(int i)
{
return m_components[i]->get();
};
std::vector<double> get() const
{
std::vector<double> val;
for (unsigned int i=0;i<getSize();i++) val.push_back(m_components[i]->get());
return val;
}
void get(double * array) const
{
for (unsigned int i=0;i<getSize();i++) array[i] = m_components[i]->get();
}
double getDeriv(int i)
{
return m_components[i]->getDeriv();
};
std::vector<double> getDeriv() const
{
std::vector<double> val;
for (unsigned int i=0;i<getSize();i++) val.push_back(m_components[i]->getDeriv());
return val;
}
void getDeriv(double * array) const
{
for (unsigned int i=0;i<getSize();i++) array[i] = m_components[i]->getDeriv();
}
void set(std::vector<double> vals)
{
for (unsigned int i=0;i<getSize();i++) m_components[i]->set(vals[i]);
m_stateSpace->run();
}
void set(double * array)
{
for (unsigned int i=0;i<getSize();i++) m_components[i]->set(array[i]);
m_stateSpace->run();
}
std::string getName(int i) const
{
return m_components[i]->getName();
};
std::vector<std::string> getName() const
{
std::vector<std::string> name;
for (unsigned int i=0;i<getSize();i++) name.push_back(m_components[i]->getName());
return name;
}
std::string getUnit(int i) const
{
return m_components[i]->getUnit();
};
std::vector<std::string> getUnit() const
{
std::vector<std::string> unit;
for (unsigned int i=0;i<getSize();i++) unit.push_back(m_components[i]->getUnit());
return unit;
}
void clear() {
m_components.clear();
}
private:
FGStateSpace * m_stateSpace;
FGFDMExec * m_fdm;
std::vector<Component *> m_components;
};
// component vectors
ComponentVector x, u, y;
// constructor
FGStateSpace(FGFDMExec * fdm) : x(fdm,this), u(fdm,this), y(fdm,this), m_fdm(fdm) {};
void setFdm(FGFDMExec * fdm) { m_fdm = fdm; }
void run() {
// initialize
m_fdm->Initialize(m_fdm->GetIC());
for (unsigned int i=0; i<m_fdm->GetPropulsion()->GetNumEngines(); i++) {
m_fdm->GetPropulsion()->GetEngine(i)->InitRunning();
}
// wait for stable state
double cost = stateSum();
for(int i=0;i<1000;i++) {
m_fdm->GetPropulsion()->GetSteadyState();
m_fdm->SetTrimStatus(true);
m_fdm->DisableOutput();
m_fdm->SuspendIntegration();
m_fdm->Run();
m_fdm->SetTrimStatus(false);
m_fdm->EnableOutput();
m_fdm->ResumeIntegration();
double costNew = stateSum();
double dcost = fabs(costNew - cost);
if (dcost < std::numeric_limits<double>::epsilon()) {
if(m_fdm->GetDebugLevel() > 1) {
std::cout << "cost convergd, i: " << i << std::endl;
}
break;
}
if (i > 1000) {
if(m_fdm->GetDebugLevel() > 1) {
std::cout << "cost failed to converge, dcost: "
<< std::scientific
<< dcost << std::endl;
}
break;
}
cost = costNew;
}
}
double stateSum() {
double sum = 0;
for (unsigned int i=0;i<x.getSize();i++) sum += x.get(i);
return sum;
}
void clear() {
x.clear();
u.clear();
y.clear();
}
// deconstructor
virtual ~FGStateSpace() {};
// linearization function
void linearize(std::vector<double> x0, std::vector<double> u0, std::vector<double> y0,
std::vector< std::vector<double> > & A,
std::vector< std::vector<double> > & B,
std::vector< std::vector<double> > & C,
std::vector< std::vector<double> > & D);
private:
// compute numerical jacobian of a matrix
void numericalJacobian(std::vector< std::vector<double> > & J, ComponentVector & y,
ComponentVector & x, const std::vector<double> & y0,
const std::vector<double> & x0, double h=1e-5, bool computeYDerivative = false);
// flight dynamcis model
FGFDMExec * m_fdm;
public:
// components
class Vt : public Component
{
public:
Vt() : Component("Vt","ft/s") {};
double get() const
{
return m_fdm->GetAuxiliary()->GetVt();
}
void set(double val)
{
m_fdm->GetIC()->SetVtrueFpsIC(val);
}
double getDeriv() const
{
return (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetAccelerations()->GetUVWdot(1) +
m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetAccelerations()->GetUVWdot(2) +
m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetAccelerations()->GetUVWdot(3))/
m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot
}
};
class VGround : public Component
{
public:
VGround() : Component("VGround","ft/s") {};
double get() const
{
return m_fdm->GetAuxiliary()->GetVground();
}
void set(double val)
{
m_fdm->GetIC()->SetVgroundFpsIC(val);
}
};
class AccelX : public Component
{
public:
AccelX() : Component("AccelX","ft/s^2") {};
double get() const
{
return m_fdm->GetAuxiliary()->GetPilotAccel(1);
}
void set(double val)
{
// XXX: not possible to implement currently
}
};
class AccelY : public Component
{
public:
AccelY() : Component("AccelY","ft/s^2") {};
double get() const
{
return m_fdm->GetAuxiliary()->GetPilotAccel(2);
}
void set(double val)
{
// XXX: not possible to implement currently
}
};
class AccelZ : public Component
{
public:
AccelZ() : Component("AccelZ","ft/s^2") {};
double get() const
{
return m_fdm->GetAuxiliary()->GetPilotAccel(3);
}
void set(double val)
{
// XXX: not possible to implement currently
}
};
class Alpha : public Component
{
public:
Alpha() : Component("Alpha","rad") {};
double get() const
{
return m_fdm->GetAuxiliary()->Getalpha();
}
void set(double val)
{
double beta = m_fdm->GetIC()->GetBetaDegIC();
double psi = m_fdm->GetIC()->GetPsiRadIC();
double theta = m_fdm->GetIC()->GetThetaRadIC();
m_fdm->GetIC()->SetAlphaRadIC(val);
m_fdm->GetIC()->SetBetaRadIC(beta);
m_fdm->GetIC()->SetPsiRadIC(psi);
m_fdm->GetIC()->SetThetaRadIC(theta);
}
double getDeriv() const
{
return m_fdm->GetAuxiliary()->Getadot();
}
};
class Theta : public Component
{
public:
Theta() : Component("Theta","rad") {};
double get() const
{
return m_fdm->GetPropagate()->GetEuler(2);
}
void set(double val)
{
m_fdm->GetIC()->SetFlightPathAngleRadIC(val-m_fdm->GetIC()->GetAlphaRadIC());
//m_fdm->GetIC()->SetThetaRadIC(val);
}
double getDeriv() const
{
return m_fdm->GetAuxiliary()->GetEulerRates(2);
}
};
class Q : public Component
{
public:
Q() : Component("Q","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQR(2);
}
void set(double val)
{
m_fdm->GetIC()->SetQRadpsIC(val);
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(2);
}
};
class Alt : public Component
{
public:
Alt() : Component("Alt","ft") {};
double get() const
{
return m_fdm->GetPropagate()->GetAltitudeASL();
}
void set(double val)
{
m_fdm->GetIC()->SetAltitudeASLFtIC(val);
}
double getDeriv() const
{
return m_fdm->GetPropagate()->Gethdot();
}
};
class Beta : public Component
{
public:
Beta() : Component("Beta","rad") {};
double get() const
{
return m_fdm->GetAuxiliary()->Getbeta();
}
void set(double val)
{
double psi = m_fdm->GetIC()->GetPsiRadIC();
m_fdm->GetIC()->SetBetaRadIC(val);
m_fdm->GetIC()->SetPsiRadIC(psi);
}
double getDeriv() const
{
return m_fdm->GetAuxiliary()->Getbdot();
}
};
class Phi : public Component
{
public:
Phi() : Component("Phi","rad") {};
double get() const
{
return m_fdm->GetPropagate()->GetEuler(1);
}
void set(double val)
{
m_fdm->GetIC()->SetPhiRadIC(val);
}
double getDeriv() const
{
return m_fdm->GetAuxiliary()->GetEulerRates(1);
}
};
class P : public Component
{
public:
P() : Component("P","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQR(1);
}
void set(double val)
{
m_fdm->GetIC()->SetPRadpsIC(val);
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(1);
}
};
class R : public Component
{
public:
R() : Component("R","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQR(3);
}
void set(double val)
{
m_fdm->GetIC()->SetRRadpsIC(val);
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(3);
}
};
class Psi : public Component
{
public:
Psi() : Component("Psi","rad") {};
double get() const
{
return m_fdm->GetPropagate()->GetEuler(3);
}
void set(double val)
{
m_fdm->GetIC()->SetPsiRadIC(val);
}
double getDeriv() const
{
return m_fdm->GetAuxiliary()->GetEulerRates(3);
}
};
class ThrottleCmd : public Component
{
public:
ThrottleCmd() : Component("ThtlCmd","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetThrottleCmd(0);
}
void set(double val)
{
for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
m_fdm->GetFCS()->SetThrottleCmd(i,val);
m_fdm->GetFCS()->Run(true);
}
};
class ThrottlePos : public Component
{
public:
ThrottlePos() : Component("ThtlPos","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetThrottlePos(0);
}
void set(double val)
{
for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
m_fdm->GetFCS()->SetThrottlePos(i,val);
}
};
class DaCmd : public Component
{
public:
DaCmd() : Component("DaCmd","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDaCmd();
}
void set(double val)
{
m_fdm->GetFCS()->SetDaCmd(val);
m_fdm->GetFCS()->Run(true);
}
};
class DaPos : public Component
{
public:
DaPos() : Component("DaPos","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDaLPos();
}
void set(double val)
{
m_fdm->GetFCS()->SetDaLPos(ofRad,val);
m_fdm->GetFCS()->SetDaRPos(ofRad,val); // TODO: check if this is neg.
}
};
class DeCmd : public Component
{
public:
DeCmd() : Component("DeCmd","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDeCmd();
}
void set(double val)
{
m_fdm->GetFCS()->SetDeCmd(val);
m_fdm->GetFCS()->Run(true);
}
};
class DePos : public Component
{
public:
DePos() : Component("DePos","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDePos();
}
void set(double val)
{
m_fdm->GetFCS()->SetDePos(ofRad,val);
}
};
class DrCmd : public Component
{
public:
DrCmd() : Component("DrCmd","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDrCmd();
}
void set(double val)
{
m_fdm->GetFCS()->SetDrCmd(val);
m_fdm->GetFCS()->Run(true);
}
};
class DrPos : public Component
{
public:
DrPos() : Component("DrPos","norm") {};
double get() const
{
return m_fdm->GetFCS()->GetDrPos();
}
void set(double val)
{
m_fdm->GetFCS()->SetDrPos(ofRad,val);
}
};
class Rpm0 : public Component
{
public:
Rpm0() : Component("Rpm0","rev/min") {};
double get() const
{
return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetRPM();
}
void set(double val)
{
m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->SetRPM(val);
}
};
class Rpm1 : public Component
{
public:
Rpm1() : Component("Rpm1","rev/min") {};
double get() const
{
return m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->GetRPM();
}
void set(double val)
{
m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->SetRPM(val);
}
};
class Rpm2 : public Component
{
public:
Rpm2() : Component("Rpm2","rev/min") {};
double get() const
{
return m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->GetRPM();
}
void set(double val)
{
m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->SetRPM(val);
}
};
class Rpm3 : public Component
{
public:
Rpm3() : Component("Rpm3","rev/min") {};
double get() const
{
return m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->GetRPM();
}
void set(double val)
{
m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->SetRPM(val);
}
};
class PropPitch : public Component
{
public:
PropPitch() : Component("Prop Pitch","deg") {};
double get() const
{
return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetPitch();
}
void set(double val)
{
for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
m_fdm->GetPropulsion()->GetEngine(i)->GetThruster()->SetPitch(val);
}
};
class Longitude : public Component
{
public:
Longitude() : Component("Longitude","rad") {};
double get() const
{
return m_fdm->GetPropagate()->GetLongitude();
}
void set(double val)
{
m_fdm->GetIC()->SetLongitudeRadIC(val);
}
double getDeriv() const
{
return m_fdm->GetPropagate()->GetVel(2)/(cos(m_fdm->GetPropagate()->GetLatitude())*m_fdm->GetPropagate()->GetRadius());
}
};
class Latitude : public Component
{
public:
Latitude() : Component("Latitude","rad") {};
double get() const
{
return m_fdm->GetPropagate()->GetLatitude();
}
void set(double val)
{
m_fdm->GetIC()->SetLatitudeRadIC(val);
}
double getDeriv() const
{
return m_fdm->GetPropagate()->GetVel(1)/(m_fdm->GetPropagate()->GetRadius());
}
};
class Pi : public Component
{
public:
Pi() : Component("P inertial","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQRi(1);
}
void set(double val)
{
//Set PQR from PQRi
//VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
m_fdm->GetIC()->SetQRadpsIC(val + \
(m_fdm->GetPropagate()->GetPQR(1) - m_fdm->GetPropagate()->GetPQRi(1)));
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(1);
}
};
class Qi : public Component
{
public:
Qi() : Component("Q inertial","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQRi(2);
}
void set(double val)
{
//Set PQR from PQRi
//VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
m_fdm->GetIC()->SetQRadpsIC(val + \
(m_fdm->GetPropagate()->GetPQR(2) - m_fdm->GetPropagate()->GetPQRi(2)));
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(2);
}
};
class Ri : public Component
{
public:
Ri() : Component("R inertial","rad/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetPQRi(3);
}
void set(double val)
{
//Set PQR from PQRi
//VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
m_fdm->GetIC()->SetQRadpsIC(val + \
(m_fdm->GetPropagate()->GetPQR(3) - m_fdm->GetPropagate()->GetPQRi(3)));
}
double getDeriv() const
{
return m_fdm->GetAccelerations()->GetPQRdot(3);
}
};
class Vn : public Component
{
public:
Vn() : Component("Vel north","feet/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetVel(1);
}
void set(double val)
{
m_fdm->GetIC()->SetVNorthFpsIC(val);
}
double getDeriv() const
{
//get NED accel from body accel
return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1);
}
};
class Ve : public Component
{
public:
Ve() : Component("Vel east","feet/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetVel(2);
}
void set(double val)
{
m_fdm->GetIC()->SetVEastFpsIC(val);
}
double getDeriv() const
{
//get NED accel from body accel
return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2);
}
};
class Vd : public Component
{
public:
Vd() : Component("Vel down","feet/s") {};
double get() const
{
return m_fdm->GetPropagate()->GetVel(3);
}
void set(double val)
{
m_fdm->GetIC()->SetVDownFpsIC(val);
}
double getDeriv() const
{
//get NED accel from body accel
return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(3);
}
};
class COG : public Component
{
public:
COG() : Component("Course Over Ground","rad") {};
double get() const
{
//cog = atan2(Ve,Vn)
return atan2(m_fdm->GetPropagate()->GetVel(2),m_fdm->GetPropagate()->GetVel(1));
}
void set(double val)
{
//set Vn and Ve according to vGround and COG
m_fdm->GetIC()->SetVNorthFpsIC(m_fdm->GetAuxiliary()->GetVground()*cos(val));
m_fdm->GetIC()->SetVEastFpsIC(m_fdm->GetAuxiliary()->GetVground()*sin(val));
}
double getDeriv() const
{
double Vn = m_fdm->GetPropagate()->GetVel(1);
double Vndot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1);
double Ve = m_fdm->GetPropagate()->GetVel(2);
double Vedot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2);
//dCOG/dt = dCOG/dVe*dVe/dt + dCOG/dVn*dVn/dt
return Vn/(Vn*Vn+Ve*Ve)*Vedot - Ve/(Vn*Vn+Ve*Ve)*Vndot;
}
};
};
// stream output
std::ostream &operator<<(std::ostream &out, const FGStateSpace::Component &c );
std::ostream &operator<<(std::ostream &out, const FGStateSpace::ComponentVector &v );
std::ostream &operator<<(std::ostream &out, const FGStateSpace &ss );
std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d );
std::ostream &operator<<( std::ostream &out, const std::vector<double> &vec );
} // JSBSim
#endif
// vim:ts=4:sw=4