.. _program_listing_file_src_navtk_inertial_MechanizationStandard.hpp:

Program Listing for File MechanizationStandard.hpp
==================================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_navtk_inertial_MechanizationStandard.hpp>` (``src/navtk/inertial/MechanizationStandard.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #pragma once
   
   #include <navtk/inertial/AidingAltData.hpp>
   #include <navtk/inertial/InertialPosVelAtt.hpp>
   #include <navtk/inertial/Mechanization.hpp>
   #include <navtk/inertial/MechanizationOptions.hpp>
   #include <navtk/inertial/StandardPosVelAtt.hpp>
   #include <navtk/tensors.hpp>
   
   namespace navtk {
   namespace inertial {
   
   class MechanizationStandard : public Mechanization {
   
   public:
       virtual ~MechanizationStandard() = default;
   
       not_null<std::shared_ptr<InertialPosVelAtt>> mechanize(
           const Vector3& dv_s,
           const Vector3& dth_s,
           double dt,
           const not_null<std::shared_ptr<InertialPosVelAtt>> pva,
           const not_null<std::shared_ptr<InertialPosVelAtt>> old_pva,
           const MechanizationOptions& mech_options,
           AidingAltData* aiding_alt_data) override;
   
   private:
       Matrix3 I3 = eye(3);
   
       // Currently hardcoded as it is in mechanization_standard, and this value
       // required to match some old unit tests we have. Needs to be parameterized
       // somehow- could become a member of mechanization options.
       double omega = 7.292115e-5;
   
       // Data elements to be stripped out of or computed from the mechanize
       // function arguments
       MechanizationOptions options;
       double lat0 = 0;
       double lon0 = 0;
       double alt0 = 0;
       double vn0  = 0;
       double ve0  = 0;
       double vd0  = 0;
       Matrix3 C_s_to_n0;
       double sin_l = 0;
       double cos_l = 0;
       double sec_l = 0;
       double tan_l = 0;
       double rn;
       double re;
       double r_zero;
       double dt;
       double dv0;
       double dv1;
       double dv2;
       double dth0;
       double dth1;
       double dth2;
       Vector3 g;
       Vector3 v_ned_prev;
   
       // Force and acceleration values for NED frame
       double fn;
       double fe;
       double fd;
       double an;
       double ae;
       double ad;
   
       // Change in position (integrated velocity) in the NED frame
       double dpn;
       double dpe;
       double dpd;
   
       // Aiding altitude scale factor
       double c1;
   
       // Intermediate variables used in propagating the DCM
       Vector3 sigma = zeros(3);
       Matrix3 A;
   
       // Containers for the completed mechanized solution elements
       Vector3 llh1  = zeros(3);
       Vector3 vned1 = zeros(3);
       Matrix3 C_s_to_n1;
   
       // Updates g member based on gravity setting in options
       void calc_grav();
   
       // Updates sigma member from delta rotation
       void calc_rot_rate();
   
       // Integrates sigma to obtain A, the DCM that rotates C_S_to_n0 to C_s_to_n1
       void calc_dcm();
   
       // Updates fn, fe and fd, the specific force elements in the NED frame, m/s^2
       void calc_force_ned();
   
       // Updates an, ae and ad, the acceleration elements in the NED frame, m/s^2
       void calc_acceleration();
   
       // Integrates acceleration to obtain new velocity vned1
       void integrate_to_velocity();
   
       // Integrates velocity to obtain new position, llh1
       void integrate_to_position();
   
       // Strip out data and perform initial calculations
       void prep(const not_null<std::shared_ptr<InertialPosVelAtt>> pva,
                 const not_null<std::shared_ptr<InertialPosVelAtt>> oldpva,
                 const Vector3& dv,
                 const Vector3& dth,
                 const double dt);
   
       // Perform the actual mechanization, updating llh1, vned1 and C_s_to_n1
       void compute(AidingAltData* aiding_alt_data);
   };
   
   }  // namespace inertial
   }  // namespace navtk