Skip to content

Commit

Permalink
Modifying the propagation to work in inertial frame.
Browse files Browse the repository at this point in the history
  • Loading branch information
jberndt committed Feb 15, 2010
1 parent 1dc301e commit a3819b9
Show file tree
Hide file tree
Showing 2 changed files with 152 additions and 83 deletions.
206 changes: 128 additions & 78 deletions src/models/FGPropagate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ using namespace std;

namespace JSBSim {

static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.46 2009/12/22 22:51:09 jberndt Exp $";
static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.47 2010/02/15 03:25:32 jberndt Exp $";
static const char *IdHdr = ID_PROPAGATE;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Expand All @@ -80,7 +80,7 @@ FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
{
Debug(0);
Name = "FGPropagate";

last3_vPQRdot.InitMatrix();
last2_vPQRdot.InitMatrix();
last_vPQRdot.InitMatrix();
Expand All @@ -96,10 +96,10 @@ FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();

last3_vLocationDot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
last3_vInertialVelocity.InitMatrix();
last2_vInertialVelocity.InitMatrix();
last_vInertialVelocity.InitMatrix();
vInertialVelocity.InitMatrix();

vOmegaLocal.InitMatrix();

Expand Down Expand Up @@ -148,10 +148,10 @@ bool FGPropagate::InitModel(void)
last_vUVWdot.InitMatrix();
vUVWdot.InitMatrix();

last3_vLocationDot.InitMatrix();
last2_vLocationDot.InitMatrix();
last_vLocationDot.InitMatrix();
vLocationDot.InitMatrix();
last3_vInertialVelocity.InitMatrix();
last2_vInertialVelocity.InitMatrix();
last_vInertialVelocity.InitMatrix();
vInertialVelocity.InitMatrix();

vOmegaLocal.InitMatrix();

Expand All @@ -174,52 +174,89 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
radInv = 1.0/VehicleRadius;

// Initialize the State Vector elements
// The local copies of the transformation matrices are for use for
// initial conditions only.

// Set the position lat/lon/radius
VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
FGIC->GetLatitudeRadIC(),
FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );

// Set the Orientation from the euler angles
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform

Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform

Ti2l = Tec2l * Ti2ec;
Tl2i = Ti2l.Transposed();

// Set the orientation from the euler angles
VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
FGIC->GetThetaRadIC(),
FGIC->GetPsiRadIC() );

VState.vQtrn.Normalize(); // Normalize the quaternion.

// Normalize the quaternion.
VState.vQtrn.Normalize();
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform

Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform

Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform

// Assume at this time that the earth position angle is zero at initialization. That is,
// that the ECI and ECEF frames are coincident at initialization.
VState.vQtrni = VState.vQtrn;

// Set the velocities in the instantaneus body frame
VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
FGIC->GetVBodyFpsIC(),
FGIC->GetWBodyFpsIC() );

// Set the angular velocities in the instantaneus body frame.
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() );
VState.vInertialPosition = Tec2i * VState.vLocation;

// These local copies of the transformation matrices are for use for
// initial conditions only.
// Compute the local frame ECEF velocity
vVel = Tb2l*VState.vUVW;
vOmegaLocal.InitMatrix( radInv*vVel(eEast),
-radInv*vVel(eNorth),
-radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );

Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
// Set the angular velocities of the body frame relative to the ECEF frame,
// expressed in the body frame.
VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
FGIC->GetQRadpsIC(),
FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;

VState.vInertialPosition = Tec2i * VState.vLocation;
VState.vPQRi = VState.vPQR + Ti2b*vOmega;

// Set the inertial velocity
VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmega * VState.vInertialPosition);
// Make an initial run and set past values
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateInertialVelocity(); // Translational position derivative

// Compute the local frame ECEF velocity
vVel = GetTb2l()*VState.vUVW;
last3_vPQRdot
= last2_vPQRdot
= last_vPQRdot
= vPQRdot;

last3_vUVWdot
= last2_vUVWdot
= last_vUVWdot
= vUVWdot;

last3_vQtrndot
= last2_vQtrndot
= last_vQtrndot
= vQtrndot;

last3_vInertialVelocity
= last2_vInertialVelocity
= last_vInertialVelocity
= VState.vInertialVelocity;

// Recompute the LocalTerrainRadius.
RecomputeLocalTerrainRadius();
Expand Down Expand Up @@ -247,6 +284,8 @@ Inertial.

bool FGPropagate::Run(void)
{
static int ctr;

if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
if (FDMExec->Holding()) return false;

Expand All @@ -261,48 +300,48 @@ bool FGPropagate::Run(void)

// These local copies of the transformation matrices are for use this
// pass through Run() only.

Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
Tl2b = GetTl2b(); // local to body frame transform
Tb2l = Tl2b.Transposed(); // body to local frame transform
Tl2ec = GetTl2ec(); // local to ECEF transform
Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
Ti2ec = GetTi2ec(); // ECI to ECEF transform
Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
Ti2b = Tec2b*Ti2ec; // ECI to body frame transform
Tb2i = Ti2b.Transposed(); // body to ECI frame transform
Ti2l = Tec2l * Ti2ec;
Tl2i = Ti2l.Transposed();

// Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
vVel = Tb2l * VState.vUVW;

// Calculate state derivatives
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateLocationdot(); // Translational position derivative
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateQuatdot(); // Angular orientation derivative
CalculateInertialVelocity(); // Translational position derivative

// Integrate to propagate the state

double dt = State->Getdt()*rate; // The 'stepsize'

// Propagate rotational velocity

switch(integrator_rotational_rate) {
case eRectEuler: VState.vPQR += dt*vPQRdot;
case eRectEuler: VState.vPQRi += dt*vPQRdot;
break;
case eTrapezoidal: VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
case eTrapezoidal: VState.vPQRi += 0.5*dt*(vPQRdot + last_vPQRdot);
break;
case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
case eAdamsBashforth2: VState.vPQRi += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
break;
case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
case eAdamsBashforth3: VState.vPQRi += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
break;
case eAdamsBashforth4: VState.vPQR += (1/24.0)*dt*(55.0*vPQRdot - 59.0*last_vPQRdot + 37.0*last2_vPQRdot - 9.0*last3_vPQRdot);
case eAdamsBashforth4: VState.vPQRi += (1/24.0)*dt*(55.0*vPQRdot - 59.0*last_vPQRdot + 37.0*last2_vPQRdot - 9.0*last3_vPQRdot);
break;
case eNone: // do nothing
break;
}

// Propagate translational velocity

switch(integrator_translational_rate) {
Expand Down Expand Up @@ -340,23 +379,30 @@ bool FGPropagate::Run(void)
// Propagate translational position

switch(integrator_translational_position) {
case eRectEuler: VState.vLocation += dt*vLocationDot;
case eRectEuler: VState.vInertialPosition += dt*VState.vInertialVelocity;
break;
case eTrapezoidal: VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
case eTrapezoidal: VState.vInertialPosition += 0.5*dt*(VState.vInertialVelocity + last_vInertialVelocity);
break;
case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
case eAdamsBashforth2: VState.vInertialPosition += dt*(1.5*VState.vInertialVelocity - 0.5*last_vInertialVelocity);
break;
case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
case eAdamsBashforth3: VState.vInertialPosition += (1/12.0)*dt*(23.0*VState.vInertialVelocity - 16.0*last_vInertialVelocity + 5.0*last2_vInertialVelocity);
break;
case eAdamsBashforth4: VState.vLocation += (1/24.0)*dt*(55.0*vLocationDot - 59.0*last_vLocationDot + 37.0*last2_vLocationDot - 9.0*last3_vLocationDot);
case eAdamsBashforth4: VState.vInertialPosition += (1/24.0)*dt*(55.0*VState.vInertialVelocity - 59.0*last_vInertialVelocity + 37.0*last2_vInertialVelocity - 9.0*last3_vInertialVelocity);
break;
case eNone: // do nothing, freeze translational position
break;
}

// Normalize the quaternion
VState.vQtrn.Normalize();

// Set auxililary state variables
VState.vInertialPosition = Tec2i * VState.vLocation;
VState.vPQRi = VState.vPQR + Ti2b*vOmega;
VState.vLocation = Ti2ec*VState.vInertialPosition;

VState.vPQR = VState.vPQRi - Ti2b*vOmega;

FGQuaternion vQuatEPA(0,0,Inertial->GetEarthPositionAngle());
VState.vQtrni = VState.vQtrn*vQuatEPA;

// Set past values

Expand All @@ -372,11 +418,11 @@ bool FGPropagate::Run(void)
last2_vQtrndot = last_vQtrndot;
last_vQtrndot = vQtrndot;

last3_vLocationDot = last2_vLocationDot;
last2_vLocationDot = last_vLocationDot;
last_vLocationDot = vLocationDot;
last3_vInertialVelocity = last2_vInertialVelocity;
last2_vInertialVelocity = last_vInertialVelocity;
last_vInertialVelocity = VState.vInertialVelocity;

RunPreFunctions();
RunPostFunctions();

Debug(2);
return false;
Expand Down Expand Up @@ -415,6 +461,13 @@ void FGPropagate::CalculatePQRdot(void)
// vQtrndot is the quaternion derivative.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16b (page 50)
// See also 1.5-14a for vOmegaLocal, which is the rotational
// rate of the vehicle-carried local frame relative to the ECEF
// frame, expressed in the NED frame (North, East, Down). In JSBSim
// the NED frame is referred to as the local NED frame - or simply,
// the local frame. The Tl2b transform takes vOmegaLocal from
// being expressed in the local frame to being expressed in the
// body frame.

void FGPropagate::CalculateQuatdot(void)
{
Expand All @@ -423,7 +476,9 @@ void FGPropagate::CalculateQuatdot(void)
-radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );

// Compute quaternion orientation derivative on current body rates
vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal);
VState.vQtrn.Normalize();
vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal );
// vQtrndot = VState.vQtrni.GetQDot( VState.vPQRi);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Expand All @@ -438,14 +493,14 @@ void FGPropagate::CalculateUVWdot(void)
const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces

// Begin to compute body frame accelerations based on the current body forces
vUVWdot = vForces/mass - VState.vPQR * VState.vUVW;
vUVWdot = vForces/mass - VState.vPQRi * VState.vUVW;

// Include Coriolis acceleration.
// vOmega is the Earth angular rate - expressed in the inertial frame -
// so it has to be transformed to the body frame. More completely,
// vOmega is the rate of the ECEF frame relative to the Inertial
// frame (ECI), expressed in the Inertial frame.
vUVWdot -= 2.0 * (Ti2b *vOmega) * VState.vUVW;
vUVWdot -= (Ti2b *vOmega) * VState.vUVW;

// Include Centrifugal acceleration.
if (!GroundReactions->GetWOW()) {
Expand All @@ -460,16 +515,11 @@ void FGPropagate::CalculateUVWdot(void)

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGPropagate::CalculateLocationdot(void)
void FGPropagate::CalculateInertialVelocity(void)
{
// Transform the vehicle velocity relative to the ECEF frame, expressed
// in the body frame, to be expressed in the ECEF frame.
vLocationDot = Tb2ec * VState.vUVW;

// Now, transform the velocity vector of the body relative to the origin (Earth
// Transform the velocity vector of the body relative to the origin (Earth
// center) to be expressed in the inertial frame, and add the vehicle velocity
// contribution due to the rotation of the planet. The above velocity is only
// relative to the rotating ECEF frame.
// contribution due to the rotation of the planet.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16c (page 50)

Expand Down
Loading

0 comments on commit a3819b9

Please sign in to comment.