Simulate the fast reorientation system and demonstrate quaternion propagation

The routines are: QProp, QIToBDot, and the JPL propagation from the Wong reference.

------------------------------------------------------------------------------- References: Wong, E.C. and W.G. Breckenridge, "Inertial Attitude Determination for a Dual-Spin Planetary Spacecraft", AIAA Journal of Guidance Navigation, and Control Vol. 6, No. 6, Nov-Dec 1983, pp. 491-8. ------------------------------------------------------------------------- See also FRSTarg, FRSMPlan, FRSCGen, ShapeFlt, QIToBDot, QMult, QPose, QTForm, Plot2D, TimeGUI, RK4, Unit, QMultJPL, QProp, RIGModel -------------------------------------------------------------------------

Global for the TimeGUI
Simulation time step
The spacecraft
Design the shaping filter
Initialize the state vector
Set up the parameters
Compute the quaternion that will rotate the spacecraft to the target
The maximum allowable acceleration in rad/sec^2
Gyro Data
PD Controller
Plotting variables
Start the maneuver
One step to initialize quaternion propagation
FRS Command generator
Spacecraft PD
Add the FRS torque and pd torque
Simulation
Initialize the time display
Plot the results

%--------------------------------------------------------------------------
%   Copyright (c) 1997-1998 Princeton Satellite Systems, Inc.
%   All rights reserved.
%--------------------------------------------------------------------------
%   Since version 2.
%--------------------------------------------------------------------------

Global for the TimeGUI

%------------------------
global simulationAction
simulationAction = ' ';

Simulation time step

%---------------------
dT   = 0.5;
n    = 5;
nSim = 350;

The spacecraft

%---------------
cM          = [ 0 0  0 ]';
mass        = 1000.0;
inertia     = [ 2300 0 0; 0 4500 0; 0 0 6600];
invInertia  = inv(inertia);
torqueExt   = zeros(3,1);
forceExt    = zeros(3,1);

Design the shaping filter

%--------------------------
wn        = [0.4835 0.9781 1.2437];
wnw       = [0.5    0.5    0.5   ];
wD        = [-40 -40 -40];

[af,bf,cf,df] = ShapeFlt( wn, wnw, wD, 0.2, dT );
xf            = zeros(length(af),1);

Initialize the state vector

%----------------------------
x      = [ 0.5 -0.5 0.5 -0.5   0.0 0.01 0.0 ]';
% x      = [ 1 0 0 0   0.0 0.3 0.0 ]';

Set up the parameters

%----------------------
qRefTo0      = x(1:4) ;
xmodel       = zeros(2,1);
xGyro        = zeros(6,1);

utargetref   = Unit([ 0.1  0.5 -0.4]');
usensor0     = [1;0;0];

Compute the quaternion that will rotate the spacecraft to the target

%---------------------------------------------------------------------
qRefTo1      = FRSTarg( qRefTo0, usensor0, utargetref );

The maximum allowable acceleration in rad/sec^2

%------------------------------------------------
maxaccel     = 0.006;

[qRefTo0i, axis0, maxaccel, nhalf, tbbscale, xmodel, umnvr] = ...
    FRSMPlan( qRefTo0, maxaccel, dT, inertia, qRefTo1 );

Gyro Data

%----------

uGyro                 = eye(3);
rateBiasGyro          = [0; 0; 0];
rateRandomWalk1Sigma  = [0; 0; 0];
betaGyro              = [0; 0; 0];
noiseRandomWalk1Sigma = [0; 0; 0];

PD Controller

%--------------
rateGain     = 2.0*0.7071*0.1*diag(inertia);
angleGain    =      2.0*0.1^2*diag(inertia);

Plotting variables

%-------------------
anglePlot = zeros(3,nSim);
ratePlot  = zeros(1,nSim);
errorPlot = zeros(1,nSim);
q0Plot    = zeros(4,nSim);
tPlot     = zeros(1,nSim);
uMnvrPlot = zeros(2,nSim);
wPlot     = zeros(3,nSim);
tCPLot    = zeros(3,nSim);

gyroPlot  = zeros(3,nSim);
qPSSPlot  = zeros(4,nSim);
qJPLPlot  = zeros(4,nSim);
qPSS2Plot = zeros(4,nSim);

Start the maneuver

%-------------------
t = 0;
dTSim = dT/n;

One step to initialize quaternion propagation

%----------------------------------------------

FRS Command generator

%----------------------
[qRefTo0, modelrate, tFRS, xmodel, umnvrf, umnvr, xf] = FRSCGen( axis0,...
    af, bf, cf, df, xmodel, tbbscale, qRefTo0i, umnvr, xf, dT, nhalf,...
    x(5:7), inertia, maxaccel, i );

Spacecraft PD

%--------------
angleError        = QMult( QPose(x(1:4)), qRefTo0 );
rateError         = x(5:7) - modelrate;
pd                = - rateGain.*rateError - angleGain.*angleError(2:4);

Add the FRS torque and pd torque

%---------------------------------
torqueExt = tFRS + pd;

Simulation

%-----------
for i=1:2
  x         = RK4( 'FRB', x, dTSim, t, inertia, invInertia, torqueExt );

  xGyroLast = xGyro;
  xGyroDot  = RIGModel(uGyro, x(5:7), rateBiasGyro, rateRandomWalk1Sigma,...
      noiseRandomWalk1Sigma, betaGyro);
  xGyro     = xGyroDot*dTSim;
  t         = t + dTSim;
end;

qPSS  = x(1:4);
qJPL  = [-x(2:4);x(1)];
qPSS2 = x(1:4);

Initialize the time display

%----------------------------
tToGoMem.lastJD        = 0;
tToGoMem.lastStepsDone = 0;
tToGoMem.kAve          = 0;
ratioRealTime          = 0;
[ ratioRealTime, tToGoMem ] =  TimeGUI( nSim, 0, tToGoMem, 0, dTSim, 'QuatProp' );

for i = 1:nSim,

  % Display the status message
  %---------------------------
  [ ratioRealTime, tToGoMem ] = TimeGUI( nSim, i, tToGoMem, ratioRealTime, dTSim );

  % FRS Command generator
  %----------------------
  [qRefTo0, modelrate, tFRS, xmodel, umnvrf, umnvr, xf] = FRSCGen( axis0,...
      af, bf, cf, df, xmodel, tbbscale, qRefTo0i, umnvr, xf, dT, nhalf,...
      x(5:7), inertia, maxaccel, i );

  % Spacecraft PD
  %--------------
  angleError        = QMult( QPose(x(1:4)), qRefTo0 );
  rateError         = x(5:7) - modelrate;
  pd                = - rateGain.*rateError - angleGain.*angleError(2:4);

  % Quantities for output of the simulation
  %----------------------------------------
  uMnvrPlot(:,i)    = [umnvrf;umnvr];
  anglePlot(:,i)    = angleError(2:4);
  ratePlot(i)       = axis0'*x(5:7) ;
  tPlot(i)          = t;
  q0Plot(:,i)       = x(1:4);
  wPlot(:,i)        = x(5:7);
  tCPlot(:,i)       = tFRS;
  errorPlot(i)      = acos( utargetref' * QTForm( qRefTo0, usensor0 ) );

  qPSSPlot(:,i)     = qPSS;
  qPSS2Plot(:,i)    = qPSS2;
  qJPLPlot(:,i)     = [qJPL(4); -qJPL(1:3)];
  gyroPlot(:,i)     = xGyro(1:3);

  % Add the FRS torque and pd torque
  %---------------------------------
  torqueExt = tFRS + pd;

  % Simulation
  %-----------
  for k = 1:n

    qPSS     = QProp( qPSS, [xGyro(1:3) xGyroLast(1:3)], 4 );
    qPSS2    = Unit(qPSS2+QIToBDot( qPSS2, xGyroDot(1:3), dTSim)*dTSim);

    thetaLastJPL = [xGyroLast(1:3); 0];
    thetaJPL     = [xGyro(1:3); 0];
    thetaJPLSq   = QMultJPL(thetaJPL)*thetaJPL;
    thetaJPLCu   = QMultJPL(thetaJPLSq)*thetaJPL;
    thetaJPLQu   = QMultJPL(thetaJPLCu)*thetaJPL;

    qJPLFact  = [0;0;0;1] + 0.5*thetaJPL + 0.125*thetaJPLSq + (thetaJPLCu ...
        + QMultJPL(thetaLastJPL)*thetaJPL - QMultJPL(thetaJPL)*thetaLastJPL)/48 ...
        + thetaJPLQu/384;

    qJPL      = QMultJPL(qJPLFact)*qJPL;

    x         = RK4( 'FRB', x, dTSim, t, inertia, invInertia, torqueExt );

    xGyroLast = xGyro;
    xGyroDot  = RIGModel(uGyro, x(5:7), rateBiasGyro, rateRandomWalk1Sigma,...
        noiseRandomWalk1Sigma, betaGyro);
    xGyro     = xGyroDot*dTSim;
    t         = t + dTSim;
  end

  % Time control
  %-------------
  switch simulationAction
    case 'pause'
      pause
      simulationAction = ' ';
    case 'stop'
      return;
    case 'plot'
      break;
  end

end


j     = 1:i;
tPlot = tPlot(j);

Plot the results

%-----------------
Plot2D( tPlot, q0Plot(:,j),    'Time (sec)', 'Quaternion',      'Quaternion'       )
Plot2D( tPlot, qPSSPlot(:,j),  'Time (sec)', 'Quaternion',      'QProp Quaternion'       )
Plot2D( tPlot, qPSS2Plot(:,j), 'Time (sec)', 'Quaternion',      'QIToBDot Quaternion'       )
Plot2D( tPlot, qJPLPlot(:,j),  'Time (sec)', 'Quaternion',      'JPL Quaternion'       )

Plot2D( tPlot, anglePlot(:,j), 'Time (sec)', 'Angle (rad)',     'Tracking Error'   )
Plot2D( tPlot, errorPlot(j),   'Time (sec)', 'Error (rad)',     'Maneuver Error'   )
Plot2D( tPlot, ratePlot(j),    'Time (sec)', 'Rate (rad/sec)',  'Maneuver Rate'    )
Plot2D( tPlot, uMnvrPlot(:,j), 'Time (sec)', 'Command',         'Maneuver Command' )
Plot2D( tPlot, wPlot(:,j),     'Time (sec)', 'Rates (rad/sec)', 'Body Rates'       )
Plot2D( tPlot, tCPlot(:,j),    'Time (sec)', 'Torque',          'Commanded Torque' )

TimeGUI('close');
Figui;


%--------------------------------------