Skip to content
Snippets Groups Projects
Commit f70baea0 authored by James Goppert's avatar James Goppert
Browse files

Addec C/D, realized was using x, not xdot in A/B

parent 42ee5042
No related branches found
No related tags found
No related merge requests found
......@@ -21,82 +21,94 @@
namespace JSBSim
{
void FGStateSpace::linearize(std::vector<double> x0, std::vector<double> u0,
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> > & B,
std::vector< std::vector<double> > & C,
std::vector< std::vector<double> > & D)
{
int n = x.getSize();
int p = u.getSize();
double h = 1e-3;
//std::cout.precision(10);
double h = 1e-5;
x.set(x0);
u.set(u0);
m_fdm.Setdt(h);
// A, f(x,u)/dx
A.resize(n);
for (int i=0;i<n;i++)
// A, d(x)/dx : TODO change x to xd
numericalJacobian(A,x,x,x0,x0,h);
// B, d(x)/du : TODO chagne x to xd
numericalJacobian(B,x,u,x0,u0,h);
// 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)
{
int n = x.getSize();
int m = y.getSize();
J.resize(m);
for (int i=0;i<m;i++)
{
A[i].resize(n);
J[i].resize(n);
for (int j=0;j<n;j++)
{
x.set(x0); u.set(u0);
double f1 = x.get(i);
x.set(x0); y.set(y0);
x.set(j,x.get(j)+h);
m_fdm.Run();
double f2 = x.get(i);
A[i][j] = (f2-f1)/h;
x.set(x0); u.set(u0);
//std::cout << std::scientific << "\ti:\t" << x.getName(i) << "\tj:\t"
//<< x.getName(j) << "\tf1:\t" << f1 << "\tf2:\t" << f2
//<< "\tdf:\t" << (f2-f1) << "\tdf/dx:\t" << A[i][j]
//<< std::fixed << std::endl;
}
}
double f1 = y.get(i);
// B, f(x,u)/du
B.resize(n);
for (int i=0;i<n;i++)
{
B[i].resize(p);
for (int j=0;j<p;j++)
{
x.set(x0); u.set(u0);
double f1 = x.get(i);
u.set(j,u.get(j)+h);
x.set(x0); y.set(y0);
x.set(j,x.get(j)+2*h);
m_fdm.Run();
double f2 = y.get(i);
x.set(x0); y.set(y0);
x.set(j,x.get(j)-h);
m_fdm.Run();
double f2 = x.get(i);
B[i][j] = (f2-f1)/h;
x.set(x0); u.set(u0);
//std::cout << std::scientific << "\ti:\t" << x.getName(i) << "\tj:\t"
//<< u.getName(j) << "\tf1:\t" << f1 << "\tf2:\t" << f2
//<< "\tdf:\t" << (f2-f1) << "\tdf/dx:\t" << B[i][j]
//<< std::fixed << std::endl;
double fn1 = y.get(i);
x.set(x0); y.set(y0);
x.set(j,x.get(j)-2*h);
m_fdm.Run();
double fn2 = y.get(i);
J[i][j] = (8*(f1-fn1)-(f2-fn2))/(12*h);
x.set(x0); y.set(y0);
std::cout << std::scientific << "\ti:\t" << y.getName(i) << "\tj:\t"
<< x.getName(j) << "\tf1:\t" << f1 << "\tf2:\t" << f2
<< "\tdf:\t" << (f2-f1) << "\tdf/dx:\t" << J[i][j]
<< std::fixed << std::endl;
}
}
}
ostream &operator<<( ostream &out, const FGStateSpace::Component &c )
std::ostream &operator<<( std::ostream &out, const FGStateSpace::Component &c )
{
out << "\t" << c.getName()
<< "\t" << c.getUnit()
<< "\t:\t" << c.get() << std::endl;
<< "\t:\t" << c.get() << std::ends;
}
ostream &operator<<( ostream &out, const FGStateSpace::ComponentVector &v )
std::ostream &operator<<( std::ostream &out, const FGStateSpace::ComponentVector &v )
{
for (int i=0; i< v.getSize(); i++)
{
out << *(v.getComp(i));
out << *(v.getComp(i)) << std::endl;
}
out << std::endl;
out << std::ends;
}
ostream &operator<<( ostream &out, const FGStateSpace &ss )
std::ostream &operator<<( std::ostream &out, const FGStateSpace &ss )
{
out << "\nX:\n" << ss.x << "\nU:\n" << ss.u << std::endl;
out << "\nX:\n" << ss.x
<< "\nU:\n" << ss.u
<< "\nY:\n" << ss.y
<< std::ends;
}
ostream &operator<<( ostream &out, const std::vector< std::vector<double> > &vec2d )
std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d )
{
for (int i=0;i<vec2d.size();i++)
{
......@@ -106,6 +118,7 @@ ostream &operator<<( ostream &out, const std::vector< std::vector<double> > &vec
}
out << std::endl;
}
out << std::ends;
}
......
......@@ -253,7 +253,7 @@ public:
class N1 : public Component
{
public:
N1() : Component("N1","rev/min") {};
N1() : Component("N1","% rpm") {};
double get() const
{
FGEngine * engine = m_fdm->GetPropulsion()->GetEngine(0);
......@@ -288,7 +288,7 @@ public:
class N2 : public Component
{
public:
N2() : Component("N2","rev/min") {};
N2() : Component("N2","% rpm") {};
double get() const
{
FGEngine * engine = m_fdm->GetPropulsion()->GetEngine(0);
......@@ -320,12 +320,19 @@ public:
}
}
};
FGStateSpace(FGFDMExec & fdm) : x(fdm), u(fdm), m_fdm(fdm) {};
FGStateSpace(FGFDMExec & fdm) : x(fdm), u(fdm), y(fdm), m_fdm(fdm) {};
virtual ~FGStateSpace(){};
class ComponentVector
{
public:
ComponentVector(FGFDMExec & fdm) : m_fdm(fdm), m_v() {}
ComponentVector & operator=(ComponentVector & cv)
{
m_fdm = cv.m_fdm;
m_v = cv.m_v;
return *this;
}
ComponentVector(const ComponentVector & cv) : m_fdm(cv.m_fdm), m_v(cv.m_v) {}
void add(Component * comp) {comp->setFdm(&m_fdm); m_v.push_back(comp); }
int getSize() const { return m_v.size(); }
Component * getComp(int i) const { return m_v[i]; };
......@@ -361,16 +368,23 @@ public:
FGFDMExec & m_fdm;
std::vector<Component *> m_v;
};
void linearize(std::vector<double> X0, std::vector<double> U0,
std::vector< std::vector<double> > & A, std::vector< std::vector<double> > & B);
ComponentVector x, u;
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);
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);
ComponentVector x, u, y;
private:
FGFDMExec & m_fdm;
};
ostream &operator<<( ostream &out, const FGStateSpace::Component &c );
ostream &operator<<( ostream &out, const FGStateSpace::ComponentVector &v );
ostream &operator<<( ostream &out, const FGStateSpace &ss );
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 );
} // JSBSim
......
......@@ -43,7 +43,7 @@ FGNelderMead::FGNelderMead(Function & f, const std::vector<double> & initialGues
m_showSimplex(showSimplex)
{
// setup
std::cout.precision(5);
std::cout.precision(3);
double rtolI = 0;
double minCostPrevResize = 0, minCost = 0;
double minCostPrev = 0, maxCost = 0, nextMaxCost = 0;
......@@ -812,31 +812,22 @@ int main (int argc, char const* argv[])
ss.u.add(new FGStateSpace::DePos);
ss.u.add(new FGStateSpace::DrPos);
std::vector< std::vector<double> > A,B;
// state feedback
ss.y = ss.x;
std::vector< std::vector<double> > A,B,C,D;
std::vector<double> x0 = ss.x.get(), u0 = ss.u.get();
std::vector<double> y0 = x0; // state feedback
std::cout << ss << std::endl;
ss.linearize(x0,u0,A,B);
std::cout << "\nA\n" << std::endl;
for (int i=0;i<A.size();i++)
{
for (int j=0;j<A[0].size();j++)
{
std::cout << "\t" << std::setw(15) << A[i][j];
}
std::cout << std::endl;
}
ss.linearize(x0,u0,y0,A,B,C,D);
std::cout << "\nB\n" << std::endl;
for (int i=0;i<B.size();i++)
{
for (int j=0;j<B[0].size();j++)
{
std::cout << "\t" << std::setw(15) << B[i][j];
}
std::cout << std::endl;
}
std::cout << std::scientific;
std::cout << "\nA\n" << A << std::endl;
std::cout << "\nB\n" << B << std::endl;
std::cout << "\nC\n" << C << std::endl;
std::cout << "\nD\n" << D << std::endl;
std::cout << std::fixed;
}
// vim:ts=4:sw=4
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment