コード例 #1
0
ファイル: Spacecraft.cpp プロジェクト: ianmartin/GPSTk
   // get State Transition Matrix 6*6
   Matrix<double> Spacecraft::getStateTransitionMatrix()
   {
      /* Transition Matrix
          |                  |
          | dr_dr0   dr_dv0  |
      phi=|                  |
          | dv_dr0   dv_dv0  |
          |                  |
      */
      const int np=p.size();

      Matrix<double> phiMatrix(6,6,0.0);

      /// dr/dr0
      phiMatrix(0,0)=dr_dr0(0);
      phiMatrix(0,1)=dr_dr0(1);
      phiMatrix(0,2)=dr_dr0(2);
      phiMatrix(1,0)=dr_dr0(3);
      phiMatrix(1,1)=dr_dr0(4);
      phiMatrix(1,2)=dr_dr0(5);
      phiMatrix(2,0)=dr_dr0(6);
      phiMatrix(2,1)=dr_dr0(7);
      phiMatrix(2,2)=dr_dr0(8);
      /// dr/dv0
      phiMatrix(0,3)=dr_dv0(0);
      phiMatrix(0,4)=dr_dv0(1);
      phiMatrix(0,5)=dr_dv0(2);
      phiMatrix(1,3)=dr_dv0(3);
      phiMatrix(1,4)=dr_dv0(4);
      phiMatrix(1,5)=dr_dv0(5);
      phiMatrix(2,3)=dr_dv0(6);
      phiMatrix(2,4)=dr_dv0(7);
      phiMatrix(2,5)=dr_dv0(8);
      /// dv/dr0
      phiMatrix(3,0)=dv_dr0(0);
      phiMatrix(3,1)=dv_dr0(1);
      phiMatrix(3,2)=dv_dr0(2);
      phiMatrix(4,0)=dv_dr0(3);
      phiMatrix(4,1)=dv_dr0(4);
      phiMatrix(4,2)=dv_dr0(5);
      phiMatrix(5,0)=dv_dr0(6);
      phiMatrix(5,1)=dv_dr0(7);
      phiMatrix(5,2)=dv_dr0(8);
      /// dv/dv0
      phiMatrix(3,3)=dv_dv0(0);
      phiMatrix(3,4)=dv_dv0(1);
      phiMatrix(3,5)=dv_dv0(2);
      phiMatrix(4,3)=dv_dv0(3);
      phiMatrix(4,4)=dv_dv0(4);
      phiMatrix(4,5)=dv_dv0(5);
      phiMatrix(5,3)=dv_dv0(6);
      phiMatrix(5,4)=dv_dv0(7);
      phiMatrix(5,5)=dv_dv0(8);

      return phiMatrix;

   }  // End of method 'Spacecraft::getStateTransitionMatrix()'
コード例 #2
0
ファイル: Spacecraft.cpp プロジェクト: REC-SPb-ETU/GPSTk
   void Spacecraft::setStateVector(Vector<double> y)
   {
      const int dim = y.size();
      const int np = (dim-42)/6;
      
         // resize the vectors
      p.resize(np,0.0);
      dr_dp0.resize(3*np,0.0);
      dv_dp0.resize(3*np,0.0);
      
      r(0) = y(0);
      r(1) = y(1);
      r(2) = y(2);
      v(0) = y(3);
      v(1) = y(4);
      v(2) = y(5);

      for(int i=0;i<9;i++)
      {
         dr_dr0(i) = y(6+i);
         dr_dv0(i) = y(15+i);

         dv_dr0(i) = y(24+3*np+i);
         dv_dv0(i) = y(33+3*np+i);
      }
      
      for(int i=0;i<3*np;i++)
      {
         dr_dp0(i) = y(24+i);
         dv_dp0(i) = y(42+3*np+i);
      }

   }  // End of method 'Spacecraft::setStateVector(Vector<double> y)'
コード例 #3
0
ファイル: Spacecraft.cpp プロジェクト: REC-SPb-ETU/GPSTk
   Vector<double> Spacecraft::getStateVector()
   {
      const int np=p.size();
      Vector<double> y(6*np+42);

      y(0) = r(0);
      y(1) = r(1);
      y(2) = r(2);
      y(3) = v(0);
      y(4) = v(1); 
      y(5) = v(2);
      
      for(int i=0;i<9;i++)
      {
         y(6+i) = dr_dr0(i);
         y(15+i) = dr_dv0(i);
         
         y(24+3*np+i) = dv_dr0(i);
         y(33+3*np+i) = dv_dv0(i);
      }
      for(int i=0;i<3*np;i++)
      {
         y(24+i) = dr_dp0(i);
         y(42+3*np+i) = dv_dp0(i);
      }

      return y;

   }  // End of method 'Spacecraft::getStateVector()'
コード例 #4
0
ファイル: Spacecraft.cpp プロジェクト: REC-SPb-ETU/GPSTk
   void Spacecraft::setTransitionMatrix(Matrix<double> phiMatrix)
   {
         /* Transition Matrix
            |                          |
            | dr_dr0   dr_dv0   dr_dp0 |
            |                          |
            phi=| dv_dr0   dv_dv0   dv_dp0 |
            |                          |
            | 0         0          I     |
            |                          |
         */

      const int np = phiMatrix.rows()-6;

         // resize the vectors
      p.resize(np,0.0);
      dr_dp0.resize(3*np,0.0);
      dv_dp0.resize(3*np,0.0);

         // dr/dr0
      dr_dr0(0) = phiMatrix(0,0);
      dr_dr0(1) = phiMatrix(0,1);
      dr_dr0(2) = phiMatrix(0,2);
      dr_dr0(3) = phiMatrix(1,0);
      dr_dr0(4) = phiMatrix(1,1);
      dr_dr0(5) = phiMatrix(1,2);
      dr_dr0(6) = phiMatrix(2,0);
      dr_dr0(7) = phiMatrix(2,1);
      dr_dr0(8) = phiMatrix(2,2);
         // dr/dv0
      dr_dv0(0) = phiMatrix(0,3);
      dr_dv0(1) = phiMatrix(0,4);
      dr_dv0(2) = phiMatrix(0,5);
      dr_dv0(3) = phiMatrix(1,3);
      dr_dv0(4) = phiMatrix(1,4);
      dr_dv0(5) = phiMatrix(1,5);
      dr_dv0(6) = phiMatrix(2,3);
      dr_dv0(7) = phiMatrix(2,4);
      dr_dv0(8) = phiMatrix(2,5);
         // dv/dr0
      dv_dr0(0) = phiMatrix(3,0);
      dv_dr0(1) = phiMatrix(3,1);
      dv_dr0(2) = phiMatrix(3,2);
      dv_dr0(3) = phiMatrix(4,0);
      dv_dr0(4) = phiMatrix(4,1);
      dv_dr0(5) = phiMatrix(4,2);
      dv_dr0(6) = phiMatrix(5,0);
      dv_dr0(7) = phiMatrix(5,1);
      dv_dr0(8) = phiMatrix(5,2);
         // dv/dv0
      dv_dv0(0) = phiMatrix(3,3);
      dv_dv0(1) = phiMatrix(3,4);
      dv_dv0(2) = phiMatrix(3,5);
      dv_dv0(3) = phiMatrix(4,3);
      dv_dv0(4) = phiMatrix(4,4);
      dv_dv0(5) = phiMatrix(4,5);
      dv_dv0(6) = phiMatrix(5,3);
      dv_dv0(7) = phiMatrix(5,4);
      dv_dv0(8) = phiMatrix(5,5);

         // dr/dp0
      for(int i=0;i<np;i++)
      {
         dr_dp0(i+0*np) = phiMatrix(0,i+6);
         dr_dp0(i+1*np) = phiMatrix(1,i+6);
         dr_dp0(i+2*np) = phiMatrix(2,i+6);

         dv_dp0(i+0*np) = phiMatrix(3,i+6);
         dv_dp0(i+1*np) = phiMatrix(4,i+6);
         dv_dp0(i+2*np) = phiMatrix(5,i+6);
      }

   }  // End of method 'Spacecraft::setTransitionMatrix()'
コード例 #5
0
ファイル: Spacecraft.cpp プロジェクト: REC-SPb-ETU/GPSTk
      // Get Transition Matrix
   Matrix<double> Spacecraft::getTransitionMatrix()
   {
         /* Transition Matrix
            |                          |
            | dr_dr0   dr_dv0   dr_dp0 |
            |                          |
            phi=| dv_dr0   dv_dv0   dv_dp0 |
            |                          |
            | 0         0           I    |
            |                          |
         */
      
      const int np=p.size();

      Matrix<double> phiMatrix(np+6,np+6,0.0);
      
         /// dr/dr0
      phiMatrix(0,0)=dr_dr0(0);
      phiMatrix(0,1)=dr_dr0(1);
      phiMatrix(0,2)=dr_dr0(2);
      phiMatrix(1,0)=dr_dr0(3);
      phiMatrix(1,1)=dr_dr0(4);
      phiMatrix(1,2)=dr_dr0(5);
      phiMatrix(2,0)=dr_dr0(6);
      phiMatrix(2,1)=dr_dr0(7);
      phiMatrix(2,2)=dr_dr0(8);
         /// dr/dv0
      phiMatrix(0,3)=dr_dv0(0);
      phiMatrix(0,4)=dr_dv0(1);
      phiMatrix(0,5)=dr_dv0(2);
      phiMatrix(1,3)=dr_dv0(3);
      phiMatrix(1,4)=dr_dv0(4);
      phiMatrix(1,5)=dr_dv0(5);
      phiMatrix(2,3)=dr_dv0(6);
      phiMatrix(2,4)=dr_dv0(7);
      phiMatrix(2,5)=dr_dv0(8);
         /// dv/dr0
      phiMatrix(3,0)=dv_dr0(0);
      phiMatrix(3,1)=dv_dr0(1);
      phiMatrix(3,2)=dv_dr0(2);
      phiMatrix(4,0)=dv_dr0(3);
      phiMatrix(4,1)=dv_dr0(4);
      phiMatrix(4,2)=dv_dr0(5);
      phiMatrix(5,0)=dv_dr0(6);
      phiMatrix(5,1)=dv_dr0(7);
      phiMatrix(5,2)=dv_dr0(8);
         /// dv/dv0
      phiMatrix(3,3)=dv_dv0(0);
      phiMatrix(3,4)=dv_dv0(1);
      phiMatrix(3,5)=dv_dv0(2);
      phiMatrix(4,3)=dv_dv0(3);
      phiMatrix(4,4)=dv_dv0(4);
      phiMatrix(4,5)=dv_dv0(5);
      phiMatrix(5,3)=dv_dv0(6);
      phiMatrix(5,4)=dv_dv0(7);
      phiMatrix(5,5)=dv_dv0(8);

         /// dr/dp0
      for(int i=0;i<np;i++)
      {
         phiMatrix(0,i+6) = dr_dp0(i+0*np);
         phiMatrix(1,i+6) = dr_dp0(i+1*np);
         phiMatrix(2,i+6) = dr_dp0(i+2*np);

         phiMatrix(3,i+6) = dv_dp0(i+0*np);
         phiMatrix(4,i+6) = dv_dp0(i+1*np);
         phiMatrix(5,i+6) = dv_dp0(i+2*np);

         phiMatrix(i+6,i+6) = 1.0;
         phiMatrix(i+6,i+6) = 1.0;
         phiMatrix(i+6,i+6) = 1.0;
      }

      return phiMatrix;

   }  // End of method 'Spacecraft::getTransitionMatrix()'