Demonstrate the use of CMGs. Shows how to use a pseudo inverse to distribute torques.

A pseudo-inverse minimizes the square of the gimbal rates so it penalizes large rates heavily. However, this simple distribution law does not explicitly try and stay away from singularities. ------------------------------------------------------------------------- See also CMG, CMGAMatrix -------------------------------------------------------------------------

Initialize the CMG model
Generate the CMG matrix for alpha and beta equal zero
Test it with a torque demand
This is the distribution law
Plug the rates into the model

%-------------------------------------------------------------------------------
%  	Copyright (c) 2000 Princeton Satellite Systems, Inc. All rights reserved.
%-------------------------------------------------------------------------------

Initialize the CMG model

%-------------------------
clear u;
u.h     = [1 1 1]*10000;
u.b{1}  = eye(3);
u.b{2}  = [1 0 0;0 0 -1;0 1 0]; % Rotate +90 deg about +x;
u.b{3}  = [0 1 0;-1 0 0;0 0 1]; % Rotate -90 deg about +z;
u.alpha = [0 0 0];
u.beta  = [0 0 0];

CMG( 'initialize', u );

Generate the CMG matrix for alpha and beta equal zero

%------------------------------------------------------
a = CMGAMatrix( u );

p = pinv( a );

Test it with a torque demand

%-----------------------------
torqueDemand = [1;3;-2];

This is the distribution law

%-----------------------------
rateDemand   = p*torqueDemand;
u.alphaDot   = rateDemand([1 3 5]);
u.betaDot    = rateDemand([2 4 6]);

Plug the rates into the model

%------------------------------
CMG('set gimbal angles', u );
CMG('set gimbal rates',  u );

momentum     = CMG( 'get momentum' )
torqueResult = CMG( 'get torque'   )
torqueError  = torqueResult - torqueDemand


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