text: /*
   * File: INS.c
   *
   * Code generated for Simulink model 'INS'.
   *
   * Model version                  : 1.2687
   * Simulink Coder version         : 9.0 (R2018b) 24-May-2018
   * C/C++ source code generated on : Sun Sep 27 14:54:07 2020
   *
   * Target selection: ert.tlc
   * Embedded hardware selection: ARM Compatible->ARM Cortex
   * Code generation objectives: Unspecified
   * Validation result: Not run
   */
  
  #include "INS.h"
  #ifndef UCHAR_MAX
  #include <limits.h>
  #endif
  
  #if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
  #error Code was generated for compiler with different sized uchar/char. \
  Consider adjusting Test hardware word size settings on the \
  Hardware Implementation pane to match your compiler word sizes as \
  defined in limits.h of the compiler. Alternatively, you can \
  select the Test hardware is the same as production hardware option and \
  select the Enable portable word sizes option on the Code Generation > \
  Verification pane for ERT based targets, which will disable the \
  preprocessor word size checks.
  #endif
  
  #if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
  #error Code was generated for compiler with different sized ushort/short. \
  Consider adjusting Test hardware word size settings on the \
  Hardware Implementation pane to match your compiler word sizes as \
  defined in limits.h of the compiler. Alternatively, you can \
  select the Test hardware is the same as production hardware option and \
  select the Enable portable word sizes option on the Code Generation > \
  Verification pane for ERT based targets, which will disable the \
  preprocessor word size checks.
  #endif
  
  #if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
  #error Code was generated for compiler with different sized uint/int. \
  Consider adjusting Test hardware word size settings on the \
  Hardware Implementation pane to match your compiler word sizes as \
  defined in limits.h of the compiler. Alternatively, you can \
  select the Test hardware is the same as production hardware option and \
  select the Enable portable word sizes option on the Code Generation > \
  Verification pane for ERT based targets, which will disable the \
  preprocessor word size checks.
  #endif
  
  #if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
  #error Code was generated for compiler with different sized ulong/long. \
  Consider adjusting Test hardware word size settings on the \
  Hardware Implementation pane to match your compiler word sizes as \
  defined in limits.h of the compiler. Alternatively, you can \
  select the Test hardware is the same as production hardware option and \
  select the Enable portable word sizes option on the Code Generation > \
  Verification pane for ERT based targets, which will disable the \
  preprocessor word size checks.
  #endif
  
  const INS_Out_Bus INS_rtZINS_Out_Bus = {
    0U,                                  /* timestamp */
    0.0F,                                /* phi */
    0.0F,                                /* theta */
    0.0F,                                /* psi */
  
    {
      0.0F, 0.0F, 0.0F, 0.0F }
    ,                                    /* quat */
    0.0F,                                /* p */
    0.0F,                                /* q */
    0.0F,                                /* r */
    0.0F,                                /* ax */
    0.0F,                                /* ay */
    0.0F,                                /* az */
    0.0F,                                /* vn */
    0.0F,                                /* ve */
    0.0F,                                /* vd */
    0.0F,                                /* reserved */
    0.0,                                 /* lon */
    0.0,                                 /* lat */
    0.0,                                 /* alt */
    0.0F,                                /* x_R */
    0.0F,                                /* y_R */
    0.0F,                                /* h_R */
    0.0F,                                /* h_AGL */
    0U,                                  /* flag */
    0U                                   /* status */
  } ;                                    /* INS_Out_Bus ground */
  
  /* Exported block parameters */
  struct_yEHaAuRwDY8DKCecxPQa0D INS_PARAM = {
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
  
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
  
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
  
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
  
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
  
    { 0.0F, 0.0F, 0.0F },
  
    { 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F },
    0U
  } ;                                    /* Variable: INS_PARAM
                                          * Referenced by:
                                          *   '<S314>/Constant'
                                          *   '<S318>/Acc_Bias'
                                          *   '<S318>/Acc_RotMatrix'
                                          *   '<S318>/Constant'
                                          *   '<S319>/Constant'
                                          *   '<S319>/Gyr_RotMatrix'
                                          *   '<S319>/Gyro_Bias'
                                          *   '<S324>/Acc_Bias'
                                          *   '<S324>/Acc_RotMatrix'
                                          *   '<S325>/Gyr_RotMatrix'
                                          *   '<S325>/Gyro_Bias'
                                          *   '<S333>/Mag_Bias'
                                          *   '<S333>/Mag_RotMatrix'
                                          */
  
  struct_TYt7YeNdxIDXfczXumtXXB INS_EXPORT = {
    2U,
  
    { 66, 97, 115, 101, 32, 73, 78, 83, 32, 118, 48, 46, 49, 46, 48, 0 }
  } ;                                    /* Variable: INS_EXPORT
                                          * Referenced by:
                                          *   '<S4>/Constant'
                                          *   '<S310>/Constant'
                                          *   '<S312>/Constant'
                                          *   '<S331>/Constant'
                                          *   '<S353>/Constant'
                                          *   '<S12>/Constant'
                                          *   '<S13>/Constant'
                                          *   '<S14>/Constant'
                                          *   '<S15>/Constant'
                                          *   '<S16>/Constant'
                                          *   '<S17>/Constant'
                                          *   '<S18>/Constant'
                                          *   '<S279>/Constant'
                                          *   '<S300>/Constant'
                                          */
  
  /* Block parameters (default storage) */
  Parameters_INS_T INS_P = {
    {
      0.002,
      9.8055F,
  
      { 60.779230000000005, 63.21755, 62.05575, 58.596309999999995,
        54.045790000000004, 48.856739999999995, 43.25262, 37.920370000000005,
        34.1301, 32.835730000000005, 34.00478, 37.23969, 42.237019999999994,
        48.32312, 53.9186, 57.25801, 57.874410000000005, 60.16512,
        61.929370000000006, 60.18821, 56.343129999999995, 51.62523, 46.48795,
        41.17103, 36.35827, 33.23469, 32.55359, 34.075480000000006, 37.37117,
        42.30482, 48.356139999999996, 53.90126, 57.13221, 57.727940000000004,
        59.39696, 60.466910000000006, 58.22041, 54.06797, 49.22043, 44.14452,
        39.127669999999995, 34.863510000000005, 32.42369, 32.38173, 34.38603,
        37.982099999999996, 43.036809999999996, 49.01101, 54.28157, 57.1746,
        57.60765, 58.49563, 58.851690000000005, 56.168949999999995, 51.79697,
        46.86045, 41.83443, 37.10604, 33.41812, 31.71341, 32.370740000000005,
        34.96261, 39.02841, 44.33641, 50.18516, 54.983940000000004, 57.34896,
        57.50847, 57.481739999999995, 57.090489999999996, 54.007160000000006,
        49.49566, 44.53838, 39.587830000000004, 35.15455, 32.071439999999996,
        31.172189999999997, 32.6316, 35.90661, 40.4968, 46.05509,
        51.697230000000005, 55.886669999999995, 57.603199999999994, 57.42095,
        56.37683, 55.184410000000007, 51.67694, 47.06351, 42.18021, 37.40885,
        33.335809999999995, 30.87989, 30.80768, 33.129220000000004, 37.14929,
        42.24863, 47.96924, 53.317730000000005, 56.84158, 57.87514,
        57.332800000000006, 55.20483, 53.14252, 49.12352, 44.37318,
        39.649910000000006, 35.23113, 31.66069, 29.86466, 30.57886, 33.72963,
        38.48585, 44.046980000000005, 49.83083, 54.82351, 57.700559999999996,
        58.09968, 57.23066, 53.99248, 50.99504, 46.34274, 41.35405, 36.82455,
        32.93035, 30.04024, 28.97274, 30.4355, 34.32127, 39.71361,
        45.648309999999995, 51.41257, 56.02422, 58.33204, 58.215849999999996,
        57.10226, 52.76961, 48.79978, 43.41476, 38.06814, 33.68879, 30.38394,
        28.29033, 28.04234, 30.24616, 34.72721, 40.57119, 46.76171,
        52.468309999999995, 56.742410000000007, 58.62415, 58.1728, 56.93836,
        51.56873, 46.637660000000004, 40.50165, 34.733839999999994,
        30.417270000000002, 27.634240000000002, 26.33522, 26.935509999999997,
        29.80698, 34.665440000000004, 40.72211, 47.04762, 52.72737, 56.81102,
        58.49203, 57.937169999999995, 56.733779999999996, 50.42271, 44.59689,
        37.80433, 31.67975, 27.40052, 25.04001, 24.42379, 25.72516, 29.0321,
        33.98829, 39.99862, 46.32105, 52.0315, 56.14271, 57.90432, 57.5006,
        56.48995, 49.36257, 42.75384, 35.49718, 29.23255, 25.12378,
        23.164540000000002, 23.03384, 24.68811, 28.0657, 32.837199999999996,
        38.57351, 44.74354, 50.50418, 54.82543, 56.914199999999994, 56.88544,
        56.214760000000005, 48.41695, 41.158550000000005, 33.66908, 27.5705,
        23.89245, 22.390259999999998, 22.531080000000003, 24.14087, 27.19338,
        31.5291, 36.86229, 42.79378, 48.56563, 53.13819, 55.66243,
        56.144760000000005, 55.92156, 47.6111, 39.831559999999996, 32.30125,
        26.62745, 23.5959, 22.584709999999998, 22.88125, 24.26992, 26.76649,
        30.482470000000003, 35.37434, 41.05399, 46.739470000000004, 51.43872,
        54.33689, 55.35237, 55.62998, 46.96745, 38.77508, 31.29864,
        26.140279999999997, 23.79252, 23.249560000000002, 23.73927, 25.04823,
        27.06239, 30.09272, 34.499309999999994, 39.90008, 45.37633, 50.00762,
        53.11284, 54.58786, 55.36239, 46.50675, 37.99237, 30.56127, 25.82161,
        24.050759999999997, 23.99545, 24.84822, 26.307119999999998, 28.02535,
        30.41591, 34.288239999999995, 39.3599, 44.55118, 48.97515, 52.11517,
        53.92517, 55.1419, 46.24845, 37.50432, 30.05446, 25.519119999999997,
        24.15809, 24.69992, 26.15681, 27.8775, 29.3492, 31.192970000000003,
        34.58305, 39.32863, 44.22475, 48.37525, 51.415169999999996,
        53.423629999999996, 54.99026, 46.21067, 37.35695, 29.842689999999997,
        25.275119999999998, 24.09149, 25.2697, 27.501990000000003,
        29.522419999999997, 30.73816, 32.172689999999996, 35.26603, 39.77221,
        44.37719, 48.20202, 51.041019999999996, 53.12553, 54.9254, 46.40833,
        37.61485, 30.07545, 25.31333, 23.9905, 25.56659, 28.49914, 30.85767,
        31.9448, 33.18322, 36.20051, 40.591120000000004, 44.92603, 48.39935,
        50.98816, 53.057730000000006, 54.96049, 46.8506, 38.34333,
        30.934060000000002, 25.98056, 24.23567, 25.70797, 28.93967, 31.57591,
        32.75325, 34.050059999999995, 37.159819999999996, 41.540620000000004,
        45.68012, 48.86316, 51.23496, 53.23523, 55.10229, 47.53783, 39.58681,
        32.5618, 27.619139999999998, 25.40825, 26.273799999999998, 29.15152,
        31.73867, 33.09767, 34.69698, 38.04961, 42.48467, 46.50912, 49.52415,
        51.77018, 53.66431, 55.35029, 48.45883, 41.35196, 35.00817, 30.40235,
        27.97858, 28.04551, 29.93862, 31.91618, 33.282160000000005,
        35.296440000000004, 39.0015, 43.50672, 47.451269999999994, 50.40533,
        52.60617, 54.34073, 55.69541, 49.58936, 43.59979, 38.21089,
        34.252230000000004, 31.98677, 31.44761, 32.11277, 32.98187, 33.93181,
        36.12594, 40.085519999999995, 44.65278, 48.5863, 51.57584, 53.76101,
        55.243089999999995, 56.12017, 50.89181, 46.24653, 42.013690000000004,
        38.88503, 37.02596, 36.18986, 35.78264, 35.34286, 35.394220000000004,
        37.250629999999994, 41.17817, 45.82739, 49.91889, 53.062589999999993,
        55.214169999999996, 56.32252, 56.59879, 52.31705, 49.172129999999996,
        46.19974, 43.91861, 42.46996, 41.46862, 40.21461, 38.516349999999996,
        37.41231, 38.54975, 42.2254, 46.999669999999995, 51.41416,
        54.803239999999995, 56.87301, 57.49695, 57.098870000000005,
        53.807230000000004, 52.230410000000006, 50.524620000000006, 48.97754,
        47.74412, 46.48918, 44.513690000000004, 41.73768, 39.52724, 39.89408,
        43.29032, 48.22045, 53.00234, 56.63673, 58.56743, 58.65522, 57.58429,
        55.2994, 55.260940000000005, 54.73759, 53.74834, 52.46873, 50.76515,
        48.097989999999996, 44.45867, 41.35911, 41.08628, 44.28579, 49.39578,
        54.500150000000005, 58.312349999999995, 60.07462, 59.6722, 58.0192,
        56.73062, 58.10316, 58.59363, 57.96544, 56.40389, 54.021080000000005,
        50.56254, 46.18894, 42.48476, 41.827510000000004, 44.969300000000004,
        50.27736, 55.62451, 59.53758, 61.16692, 60.43217, 58.372980000000005,
        58.04222, 60.610440000000004, 61.86766, 61.39109, 59.367580000000004,
        56.10736, 51.69176, 46.61971, 42.60654, 41.901720000000005, 45.12875,
        50.59406, 56.075669999999995, 60.0473, 61.66514, 60.853269999999995,
        58.624230000000004, 59.183879999999995, 62.66547, 64.38456, 63.84661,
        61.27625, 57.13352, 51.79692, 46.12612, 41.965480000000007, 41.32768,
        44.59758, 50.08132, 55.5997, 59.669230000000006, 61.48581, 60.90576,
        58.763940000000005, 60.11718, 64.1935, 66.0509, 65.2672, 62.19041,
        57.414199999999994, 51.49424, 45.44462, 41.08934, 40.25733, 43.27011,
        48.569050000000004, 54.0915, 58.396260000000005, 60.67193, 60.62021,
        58.79564, 60.817080000000004, 65.16767, 66.867770000000007, 65.71248,
        62.24969, 57.16104, 51.02143, 44.79065, 40.15217, 38.835730000000005,
        41.28144, 46.20919, 51.73817, 56.4354, 59.39162, 60.0788,
        58.734480000000005, 61.272169999999996, 65.60563, 66.91571,
        65.322510000000008, 61.589580000000005, 56.39065, 50.203720000000004,
        43.89041, 39.01946, 37.22327, 39.03303, 43.494440000000004,
        49.005019999999995, 54.16242, 57.89464, 59.39539, 58.603370000000005,
        61.48272, 65.558399999999992, 66.32146, 64.26151, 60.32188,
        55.098150000000004, 48.9392, 42.640339999999995, 37.704629999999995,
        35.64038, 36.95695, 40.96925, 46.41089, 51.979870000000005, 56.43768,
        58.68836, 58.42867, 61.45914, 65.095300000000009, 65.2265, 62.68731,
        58.550650000000005, 53.332629999999995, 47.29025, 41.16328, 36.38149,
        34.30672, 35.34609, 38.99204, 44.31616, 50.185, 55.22008, 58.05687,
        58.23604, 61.21772, 64.29179, 63.765620000000006, 60.75079, 56.40356,
        51.194019999999995, 45.344370000000005, 39.55495, 35.171870000000006,
        33.36852, 34.36254, 37.73951, 42.902519999999996, 48.94073, 54.36185,
        57.56846, 58.04636, 60.779230000000005, 63.21755, 62.05575,
        58.596309999999995, 54.045790000000004, 48.856739999999995, 43.25262,
        37.920370000000005, 34.1301, 32.835730000000005, 34.00478, 37.23969,
        42.237019999999994, 48.32312, 53.9186, 57.25801, 57.874410000000005 },
  
      { 129.8, 85.65, 47.6, 30.92, 22.28, 16.77, 13.09, 10.82, 9.61, 8.89, 7.95,
        6.4, 4.56, 3.14, 2.36, 1.63, -0.29, 117.51, 77.68, 46.33, 31.19, 22.84,
        17.29, 13.38, 10.82, 9.45, 8.96, 8.83, 8.53, 7.95, 7.45, 7.3, 7.09, 5.09,
        106.34, 71.29, 44.85, 30.96, 22.96, 17.54, 13.59, 10.82, 9.24, 8.81, 9.25,
        10.05, 10.77, 11.32, 11.89, 12.26, 10.09, 96.14, 65.84, 43.41, 30.56,
        22.81, 17.58, 13.72, 10.93, 9.32, 9.01, 9.83, 11.35, 13.0, 14.47, 15.8,
        16.81, 14.14, 86.71, 60.92, 42.06, 30.24, 22.57, 17.34, 13.59, 10.88, 9.37,
        9.27, 10.41, 12.3, 14.43, 16.53, 18.6, 20.24, 16.32, 77.87, 56.21, 40.69,
        30.09, 22.44, 16.97, 13.14, 10.5, 9.1, 9.11, 10.41, 12.45, 14.71, 17.11,
        19.72, 21.77, 14.81, 69.47, 51.43, 39.0, 29.95, 22.51, 16.68, 12.6, 9.98,
        8.62, 8.55, 9.65, 11.47, 13.52, 15.79, 18.38, 20.02, 6.11, 61.37, 46.38,
        36.6, 29.29, 22.5, 16.52, 12.2, 9.52, 8.05, 7.57, 8.0, 9.12, 10.51, 12.01,
        13.56, 12.83, -13.21, 53.48, 40.95, 33.15, 27.42, 21.58, 15.84, 11.39,
        8.48, 6.63, 5.45, 4.91, 4.97, 5.29, 5.39, 4.54, -1.26, -35.12, 45.73,
        35.12, 28.51, 23.72, 18.65, 13.24, 8.67, 5.43, 3.14, 1.37, 0.0, -0.96,
        -1.84, -3.4, -7.16, -17.97, -47.11, 38.09, 28.99, 22.82, 18.03, 12.94,
        7.52, 2.87, -0.45, -2.7, -4.44, -6.0, -7.47, -9.14, -11.86, -17.4, -29.42,
        -50.18, 30.53, 22.71, 16.44, 10.78, 4.78, -1.02, -5.48, -8.22, -9.69,
        -10.64, -11.56, -12.69, -14.36, -17.36, -23.2, -33.92, -48.41, 23.03,
        16.47, 9.97, 3.04, -4.21, -10.26, -14.03, -15.65, -15.91, -15.59, -15.23,
        -15.33, -16.43, -19.2, -24.66, -33.68, -44.07, 15.55, 10.39, 3.98, -3.89,
        -11.95, -17.69, -20.4, -20.73, -19.62, -17.89, -16.17, -15.21, -15.7,
        -18.17, -23.13, -30.66, -38.31, 8.07, 4.5, -1.2, -9.13, -17.22, -22.27,
        -23.82, -22.67, -20.02, -17.06, -14.52, -13.05, -13.21, -15.38, -19.79,
        -26.06, -31.7, 0.52, -1.29, -5.63, -12.6, -20.13, -24.55, -24.82, -21.73,
        -17.47, -13.79, -11.19, -9.79, -9.85, -11.69, -15.48, -20.57, -24.57,
        -7.14, -7.2, -9.82, -14.99, -21.41, -25.27, -23.95, -18.68, -13.2, -9.39,
        -7.14, -6.05, -6.11, -7.61, -10.7, -14.59, -17.12, -14.97, -13.48, -14.44,
        -17.43, -21.83, -24.34, -21.04, -14.31, -8.68, -5.23, -3.35, -2.46, -2.47,
        -3.58, -5.84, -8.4, -9.5, -23.01, -20.28, -20.03, -21.1, -22.59, -21.92,
        -16.23, -9.44, -4.82, -2.14, -0.59, 0.29, 0.54, 0.02, -1.15, -2.17, -1.8,
        -31.29, -27.64, -26.7, -26.56, -25.34, -20.14, -11.33, -4.94, -1.66, 0.11,
        1.26, 2.12, 2.71, 3.0, 3.22, 3.96, 5.89, -39.82, -35.46, -34.16, -33.38,
        -30.44, -21.52, -9.14, -1.96, 0.83, 2.02, 2.78, 3.5, 4.32, 5.47, 7.26, 9.9,
        13.47, -48.58, -43.57, -41.88, -40.53, -36.4, -25.5, -10.76, -1.81, 1.69,
        3.01, 3.71, 4.42, 5.54, 7.59, 10.95, 15.5, 20.86, -57.58, -51.81, -49.38,
        -47.04, -41.53, -29.62, -14.45, -4.32, 0.28, 2.24, 3.31, 4.4, 6.18, 9.29,
        14.15, 20.56, 27.91, -66.82, -60.02, -56.3, -52.29, -44.83, -31.91, -17.35,
        -7.29, -2.1, 0.41, 1.93, 3.57, 6.16, 10.34, 16.53, 24.72, 34.45, -76.32,
        -68.15, -62.42, -55.87, -45.71, -31.51, -17.83, -8.69, -3.67, -1.01, 0.73,
        2.68, 5.73, 10.52, 17.59, 27.45, 40.16, -86.13, -76.23, -67.61, -57.42,
        -43.84, -28.43, -16.0, -8.33, -4.05, -1.63, 0.08, 2.0, 4.93, 9.55, 16.73,
        27.9, 44.54, -96.33, -84.37, -71.75, -56.51, -38.99, -23.1, -12.46, -6.58,
        -3.48, -1.7, -0.34, 1.22, 3.53, 7.2, 13.37, 24.84, 46.62, -107.02, -92.82,
        -74.61, -52.35, -31.18, -16.16, -7.71, -3.67, -1.93, -1.17, -0.6, 0.16,
        1.36, 3.37, 7.31, 16.99, 44.51, -118.34, -102.02, -75.59, -43.88, -21.33,
        -9.06, -3.09, -0.69, -0.18, -0.49, -0.97, -1.34, -1.58, -1.59, -0.6, 4.89,
        34.92, -130.44, -112.81, -73.01, -30.48, -11.45, -3.46, -0.11, 0.85, 0.47,
        -0.62, -1.97, -3.42, -5.01, -6.74, -8.23, -7.18, 16.12, -143.44, -126.94,
        -61.16, -13.92, -2.99, 0.52, 1.51, 1.26, 0.26, -1.3, -3.26, -5.51, -8.02,
        -10.74, -13.41, -14.85, -3.04, -157.37, -148.26, -21.59, 1.78, 4.02, 3.97,
        3.26, 2.22, 0.82, -1.11, -3.58, -6.45, -9.49, -12.51, -15.35, -17.56,
        -13.2, -172.09, 177.85, 25.95, 13.82, 9.94, 7.48, 5.71, 4.26, 2.67, 0.53,
        -2.33, -5.62, -8.91, -11.88, -14.45, -16.66, -15.8, 172.74, 139.13, 42.46,
        21.96, 14.83, 10.83, 8.32, 6.6, 5.03, 2.96, 0.11, -3.27, -6.58, -9.36,
        -11.55, -13.5, -14.23, 157.67, 112.49, 47.27, 26.98, 18.53, 13.66, 10.61,
        8.7, 7.26, 5.56, 3.08, -0.04, -3.16, -5.64, -7.42, -9.02, -10.48, 143.23,
        96.39, 48.21, 29.72, 20.97, 15.66, 12.26, 10.21, 8.95, 7.76, 5.96, 3.41,
        0.73, -1.33, -2.65, -3.84, -5.63, 129.8, 85.65, 47.6, 30.92, 22.28, 16.77,
        13.09, 10.82, 9.61, 8.89, 7.95, 6.4, 4.56, 3.14, 2.36, 1.63, -0.29 },
  
      { -78.41, -81.01, -77.57, -71.65, -64.41, -55.01, -42.21, -25.24, -5.11,
        14.76, 31.08, 43.37, 53.08, 61.88, 70.58, 78.85, 86.01, -77.65, -79.19,
        -75.57, -69.71, -62.46, -52.92, -39.83, -22.38, -1.8, 18.1, 33.94, 45.43,
        54.33, 62.59, 71.03, 79.13, 86.11, -76.73, -77.36, -73.64, -67.84, -60.51,
        -50.81, -37.54, -19.94, 0.67, 20.42, 36.01, 47.21, 55.78, 63.75, 71.92,
        79.71, 86.36, -75.69, -75.5, -71.73, -66.0, -58.55, -48.61, -35.14, -17.51,
        2.89, 22.3, 37.65, 48.8, 57.41, 65.31, 73.2, 80.57, 86.73, -74.58, -73.59,
        -69.76, -64.15, -56.6, -46.36, -32.59, -14.9, 5.27, 24.31, 39.42, 50.55,
        59.29, 67.18, 74.78, 81.63, 87.21, -73.42, -71.6, -67.62, -62.15, -54.65,
        -44.18, -30.1, -12.29, 7.69, 26.44, 41.41, 52.56, 61.38, 69.24, 76.53,
        82.83, 87.73, -72.24, -69.56, -65.23, -59.86, -52.56, -42.09, -27.79,
        -9.83, 10.04, 28.57, 43.46, 54.68, 63.54, 71.31, 78.29, 84.06, 88.22,
        -71.09, -67.49, -62.62, -57.15, -50.1, -39.79, -25.32, -7.07, 12.75, 30.95,
        45.59, 56.73, 65.55, 73.17, 79.85, 85.12, 88.46, -69.98, -65.5, -59.91,
        -54.09, -47.08, -36.82, -22.02, -3.36, 16.28, 33.72, 47.65, 58.42, 67.07,
        74.5, 80.89, 85.71, 88.28, -68.96, -63.71, -57.41, -51.04, -43.78, -33.27,
        -18.04, 0.79, 19.76, 36.01, 48.93, 59.19, 67.63, 74.89, 81.05, 85.55,
        87.73, -68.06, -62.23, -55.45, -48.71, -41.21, -30.52, -15.25, 3.13, 21.19,
        36.43, 48.6, 58.48, 66.85, 74.13, 80.26, 84.7, 87.03, -67.28, -61.15,
        -54.37, -47.86, -40.75, -30.65, -16.27, 1.19, 18.77, 33.95, 46.17, 56.2,
        64.85, 72.43, 78.81, 83.5, 86.28, -66.64, -60.49, -54.29, -48.88, -43.16,
        -34.7, -22.14, -5.88, 11.84, 28.12, 41.62, 52.71, 62.16, 70.32, 77.13,
        82.22, 85.56, -66.15, -60.18, -55.05, -51.44, -47.76, -41.41, -31.16,
        -16.59, 1.2, 19.4, 35.46, 48.66, 59.43, 68.31, 75.55, 81.04, 84.89, -65.79,
        -60.12, -56.24, -54.69, -53.05, -48.61, -40.56, -28.0, -10.58, 9.67, 28.96,
        44.81, 57.11, 66.69, 74.25, 80.03, 84.31, -65.55, -60.16, -57.41, -57.73,
        -57.91, -55.02, -48.63, -37.57, -20.42, 1.48, 23.55, 41.77, 55.41, 65.57,
        73.32, 79.24, 83.83, -65.44, -60.23, -58.23, -59.97, -61.86, -60.43, -55.0,
        -44.25, -26.68, -3.55, 20.25, 39.94, 54.46, 64.97, 72.76, 78.68, 83.47,
        -65.46, -60.32, -58.59, -61.1, -64.58, -64.72, -59.71, -48.23, -29.52,
        -5.32, 19.31, 39.53, 54.33, 64.88, 72.54, 78.35, 83.23, -65.61, -60.48,
        -58.59, -61.11, -65.66, -67.32, -62.54, -50.03, -30.09, -5.0, 20.0, 40.22,
        54.88, 65.2, 72.59, 78.22, 83.11, -65.92, -60.8, -58.54, -60.34, -64.96,
        -67.62, -63.07, -49.92, -29.29, -3.77, 21.3, 41.36, 55.74, 65.74, 72.83,
        78.27, 83.13, -66.4, -61.42, -58.8, -59.53, -63.1, -65.72, -61.25, -47.83,
        -26.99, -1.58, 23.09, 42.7, 56.7, 66.36, 73.19, 78.49, 83.27, -67.05,
        -62.39, -59.64, -59.45, -61.44, -62.82, -57.85, -44.08, -23.09, 1.9, 25.64,
        44.35, 57.71, 66.99, 73.65, 78.88, 83.54, -67.87, -63.77, -61.22, -60.56,
        -61.18, -60.72, -54.57, -40.27, -19.06, 5.53, 28.27, 45.99, 58.68, 67.64,
        74.23, 79.45, 83.94, -68.86, -65.54, -63.48, -62.8, -62.6, -60.51, -52.99,
        -38.15, -16.82, 7.54, 29.77, 46.99, 59.39, 68.3, 74.97, 80.2, 84.45,
        -69.98, -67.64, -66.27, -65.81, -65.14, -61.83, -53.15, -37.83, -16.43,
        7.88, 30.05, 47.28, 59.82, 68.96, 75.86, 81.14, 85.08, -71.22, -70.01,
        -69.41, -69.17, -68.02, -63.66, -54.06, -38.36, -16.87, 7.54, 29.85, 47.28,
        60.11, 69.63, 76.84, 82.2, 85.8, -72.53, -72.56, -72.75, -72.54, -70.63,
        -65.21, -54.93, -39.08, -17.62, 6.92, 29.52, 47.24, 60.39, 70.25, 77.78,
        83.25, 86.56, -73.86, -75.22, -76.16, -75.65, -72.55, -65.96, -55.13,
        -39.28, -18.07, 6.4, 29.21, 47.2, 60.58, 70.68, 78.44, 84.09, 87.31,
        -75.17, -77.89, -79.53, -78.24, -73.53, -65.67, -54.2, -38.24, -17.34,
        6.66, 29.2, 47.13, 60.54, 70.67, 78.53, 84.4, 87.92, -76.4, -80.49, -82.75,
        -79.98, -73.57, -64.66, -52.62, -36.52, -15.95, 7.32, 29.17, 46.74, 59.97,
        70.02, 77.9, 84.04, 88.2, -77.48, -82.9, -85.59, -80.62, -73.05, -63.61,
        -51.43, -35.51, -15.49, 7.04, 28.31, 45.57, 58.66, 68.67, 76.65, 83.14,
        88.04, -78.35, -84.92, -87.27, -80.22, -72.23, -62.78, -50.81, -35.36,
        -16.04, 5.78, 26.6, 43.66, 56.73, 66.83, 75.06, 82.01, 87.59, -78.96,
        -86.07, -86.36, -79.06, -71.19, -61.88, -50.09, -34.93, -16.13, 4.94,
        25.07, 41.73, 54.68, 64.9, 73.46, 80.89, 87.08, -79.27, -85.82, -84.21,
        -77.46, -69.85, -60.66, -48.83, -33.59, -14.94, 5.48, 24.65, 40.53, 53.08,
        63.27, 72.09, 79.94, 86.62, -79.26, -84.51, -81.92, -75.6, -68.22, -59.05,
        -47.01, -31.39, -12.51, 7.49, 25.63, 40.43, 52.25, 62.18, 71.11, 79.26,
        86.27, -78.96, -82.81, -79.69, -73.63, -66.36, -57.1, -44.71, -28.46,
        -9.02, 10.82, 27.97, 41.49, 52.3, 61.72, 70.6, 78.89, 86.06, -78.41,
        -81.01, -77.57, -71.65, -64.41, -55.01, -42.21, -25.24, -5.11, 14.76,
        31.08, 43.37, 53.08, 61.88, 70.58, 78.85, 86.01 }
    },
    1.0E-6,
    10.0,
    1.0E-6,
    1000,
    98.055F,
    98.055F,
    -98.055F,
    -98.055F,
    0.2F,
    0.01F,
    1.0F,
    5.0F,
    38.3972435F,
    38.3972435F,
    -38.3972435F,
    -38.3972435F,
    9.3055F,
    0.0F,
    30000.0F,
    -30000.0F,
    110000.0F,
    30000.0F,
    80.0F,
    -30.0F,
    1000.0F,
    100.0F,
    1000.0F,
    1000.0F,
    1000.0F,
    200.0F,
    100.0F,
    10.3055F,
    2.5F,
    1000000U,
    200U,
    500U,
    1000U,
    5000U,
    100U,
    100U,
    500U,
    1000U,
    100U,
    100U,
    1000U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
  
    { 130, 86, 48, 31, 22, 17, 13, 11, 10, 9, 8, 6, 5, 3, 2, 2, 0, 118, 78, 46, 31,
      23, 17, 13, 11, 9, 9, 9, 9, 8, 7, 7, 7, 5, 106, 71, 45, 31, 23, 18, 14, 11,
      9, 9, 9, 10, 11, 11, 12, 12, 10, 96, 66, 43, 31, 23, 18, 14, 11, 9, 9, 10,
      11, 13, 14, 16, 17, 14, 87, 61, 42, 30, 23, 17, 14, 11, 9, 9, 10, 12, 14, 17,
      19, 20, 16, 78, 56, 41, 30, 22, 17, 13, 11, 9, 9, 10, 12, 15, 17, 20, 22, 15,
      69, 51, 39, 30, 23, 17, 13, 10, 9, 9, 10, 11, 14, 16, 18, 20, 6, 61, 46, 37,
      29, 23, 17, 12, 10, 8, 8, 8, 9, 11, 12, 14, 13, -13, 53, 41, 33, 27, 22, 16,
      11, 8, 7, 5, 5, 5, 5, 5, 5, -1, -35, 46, 35, 29, 24, 19, 13, 9, 5, 3, 1, 0,
      -1, -2, -3, -7, -18, -47, 38, 29, 23, 18, 13, 8, 3, 0, -3, -4, -6, -7, -9,
      -12, -17, -29, -50, 31, 23, 16, 11, 5, -1, -5, -8, -10, -11, -12, -13, -14,
      -17, -23, -34, -48, 23, 16, 10, 3, -4, -10, -14, -16, -16, -16, -15, -15,
      -16, -19, -25, -34, -44, 16, 10, 4, -4, -12, -18, -20, -21, -20, -18, -16,
      -15, -16, -18, -23, -31, -38, 8, 5, -1, -9, -17, -22, -24, -23, -20, -17,
      -15, -13, -13, -15, -20, -26, -32, 1, -1, -6, -13, -20, -25, -25, -22, -17,
      -14, -11, -10, -10, -12, -15, -21, -25, -7, -7, -10, -15, -21, -25, -24, -19,
      -13, -9, -7, -6, -6, -8, -11, -15, -17, -15, -13, -14, -17, -22, -24, -21,
      -14, -9, -5, -3, -2, -2, -4, -6, -8, -9, -23, -20, -20, -21, -23, -22, -16,
      -9, -5, -2, -1, 0, 1, 0, -1, -2, -2, -31, -28, -27, -27, -25, -20, -11, -5,
      -2, 0, 1, 2, 3, 3, 3, 4, 6, -40, -35, -34, -33, -30, -22, -9, -2, 1, 2, 3, 4,
      4, 5, 7, 10, 13, -49, -44, -42, -41, -36, -25, -11, -2, 2, 3, 4, 4, 6, 8, 11,
      16, 21, -58, -52, -49, -47, -42, -30, -14, -4, 0, 2, 3, 4, 6, 9, 14, 21, 28,
      -67, -60, -56, -52, -45, -32, -17, -7, -2, 0, 2, 4, 6, 10, 17, 25, 34, -76,
      -68, -62, -56, -46, -32, -18, -9, -4, -1, 1, 3, 6, 11, 18, 27, 40, -86, -76,
      -68, -57, -44, -28, -16, -8, -4, -2, 0, 2, 5, 10, 17, 28, 45, -96, -84, -72,
      -57, -39, -23, -12, -7, -3, -2, 0, 1, 4, 7, 13, 25, 47, -107, -93, -75, -52,
      -31, -16, -8, -4, -2, -1, -1, 0, 1, 3, 7, 17, 45, -118, -102, -76, -44, -21,
      -9, -3, -1, 0, 0, -1, -1, -2, -2, -1, 5, 35, -130, -113, -73, -30, -11, -3,
      0, 1, 0, -1, -2, -3, -5, -7, -8, -7, 16, -143, -127, -61, -14, -3, 1, 2, 1,
      0, -1, -3, -6, -8, -11, -13, -15, -3, -157, -148, -22, 2, 4, 4, 3, 2, 1, -1,
      -4, -6, -9, -13, -15, -18, -13, -172, 178, 26, 14, 10, 7, 6, 4, 3, 1, -2, -6,
      -9, -12, -14, -17, -16, 173, 139, 42, 22, 15, 11, 8, 7, 5, 3, 0, -3, -7, -9,
      -12, -13, -14, 158, 112, 47, 27, 19, 14, 11, 9, 7, 6, 3, 0, -3, -6, -7, -9,
      -10, 143, 96, 48, 30, 21, 16, 12, 10, 9, 8, 6, 3, 1, -1, -3, -4, -6, 130, 86,
      48, 31, 22, 17, 13, 11, 10, 9, 8, 6, 5, 3, 2, 2, 0 },
    50U,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
  
    { -78, -81, -78, -72, -64, -55, -42, -25, -5, 15, 31, 43, 53, 62, 71, 79, 86,
      -78, -79, -76, -70, -62, -53, -40, -22, -2, 18, 34, 45, 54, 63, 71, 79, 86,
      -77, -77, -74, -68, -61, -51, -38, -20, 1, 20, 36, 47, 56, 64, 72, 80, 86,
      -76, -75, -72, -66, -59, -49, -35, -18, 3, 22, 38, 49, 57, 65, 73, 81, 87,
      -75, -74, -70, -64, -57, -46, -33, -15, 5, 24, 39, 51, 59, 67, 75, 82, 87,
      -73, -72, -68, -62, -55, -44, -30, -12, 8, 26, 41, 53, 61, 69, 77, 83, 88,
      -72, -70, -65, -60, -53, -42, -28, -10, 10, 29, 43, 55, 64, 71, 78, 84, 88,
      -71, -67, -63, -57, -50, -40, -25, -7, 13, 31, 46, 57, 66, 73, 80, 85, 88,
      -70, -65, -60, -54, -47, -37, -22, -3, 16, 34, 48, 58, 67, 75, 81, 86, 88,
      -69, -64, -57, -51, -44, -33, -18, 1, 20, 36, 49, 59, 68, 75, 81, 86, 88,
      -68, -62, -55, -49, -41, -31, -15, 3, 21, 36, 49, 58, 67, 74, 80, 85, 87,
      -67, -61, -54, -48, -41, -31, -16, 1, 19, 34, 46, 56, 65, 72, 79, 84, 86,
      -67, -60, -54, -49, -43, -35, -22, -6, 12, 28, 42, 53, 62, 70, 77, 82, 86,
      -66, -60, -55, -51, -48, -41, -31, -17, 1, 19, 35, 49, 59, 68, 76, 81, 85,
      -66, -60, -56, -55, -53, -49, -41, -28, -11, 10, 29, 45, 57, 67, 74, 80, 84,
      -66, -60, -57, -58, -58, -55, -49, -38, -20, 1, 24, 42, 55, 66, 73, 79, 84,
      -65, -60, -58, -60, -62, -60, -55, -44, -27, -4, 20, 40, 54, 65, 73, 79, 83,
      -65, -60, -59, -61, -65, -65, -60, -48, -30, -5, 19, 40, 54, 65, 73, 78, 83,
      -66, -60, -59, -61, -66, -67, -63, -50, -30, -5, 20, 40, 55, 65, 73, 78, 83,
      -66, -61, -59, -60, -65, -68, -63, -50, -29, -4, 21, 41, 56, 66, 73, 78, 83,
      -66, -61, -59, -60, -63, -66, -61, -48, -27, -2, 23, 43, 57, 66, 73, 78, 83,
      -67, -62, -60, -59, -61, -63, -58, -44, -23, 2, 26, 44, 58, 67, 74, 79, 84,
      -68, -64, -61, -61, -61, -61, -55, -40, -19, 6, 28, 46, 59, 68, 74, 79, 84,
      -69, -66, -63, -63, -63, -61, -53, -38, -17, 8, 30, 47, 59, 68, 75, 80, 84,
      -70, -68, -66, -66, -65, -62, -53, -38, -16, 8, 30, 47, 60, 69, 76, 81, 85,
      -71, -70, -69, -69, -68, -64, -54, -38, -17, 8, 30, 47, 60, 70, 77, 82, 86,
      -73, -73, -73, -73, -71, -65, -55, -39, -18, 7, 30, 47, 60, 70, 78, 83, 87,
      -74, -75, -76, -76, -73, -66, -55, -39, -18, 6, 29, 47, 61, 71, 78, 84, 87,
      -75, -78, -80, -78, -74, -66, -54, -38, -17, 7, 29, 47, 61, 71, 79, 84, 88,
      -76, -80, -83, -80, -74, -65, -53, -37, -16, 7, 29, 47, 60, 70, 78, 84, 88,
      -77, -83, -86, -81, -73, -64, -51, -36, -15, 7, 28, 46, 59, 69, 77, 83, 88,
      -78, -85, -87, -80, -72, -63, -51, -35, -16, 6, 27, 44, 57, 67, 75, 82, 88,
      -79, -86, -86, -79, -71, -62, -50, -35, -16, 5, 25, 42, 55, 65, 73, 81, 87,
      -79, -86, -84, -77, -70, -61, -49, -34, -15, 5, 25, 41, 53, 63, 72, 80, 87,
      -79, -85, -82, -76, -68, -59, -47, -31, -13, 7, 26, 40, 52, 62, 71, 79, 86,
      -79, -83, -80, -74, -66, -57, -45, -28, -9, 11, 28, 41, 52, 62, 71, 79, 86,
      -78, -81, -78, -72, -64, -55, -42, -25, -5, 15, 31, 43, 53, 62, 71, 79, 86 },
    3U,
    0U,
    2U,
    1U,
  
    { 61U, 63U, 62U, 59U, 54U, 49U, 43U, 38U, 34U, 33U, 34U, 37U, 42U, 48U, 54U,
      57U, 58U, 60U, 62U, 60U, 56U, 52U, 46U, 41U, 36U, 33U, 33U, 34U, 37U, 42U,
      48U, 54U, 57U, 58U, 59U, 60U, 58U, 54U, 49U, 44U, 39U, 35U, 32U, 32U, 34U,
      38U, 43U, 49U, 54U, 57U, 58U, 58U, 59U, 56U, 52U, 47U, 42U, 37U, 33U, 32U,
      32U, 35U, 39U, 44U, 50U, 55U, 57U, 58U, 57U, 57U, 54U, 49U, 45U, 40U, 35U,
      32U, 31U, 33U, 36U, 40U, 46U, 52U, 56U, 58U, 57U, 56U, 55U, 52U, 47U, 42U,
      37U, 33U, 31U, 31U, 33U, 37U, 42U, 48U, 53U, 57U, 58U, 57U, 55U, 53U, 49U,
      44U, 40U, 35U, 32U, 30U, 31U, 34U, 38U, 44U, 50U, 55U, 58U, 58U, 57U, 54U,
      51U, 46U, 41U, 37U, 33U, 30U, 29U, 30U, 34U, 40U, 46U, 51U, 56U, 58U, 58U,
      57U, 53U, 49U, 43U, 38U, 34U, 30U, 28U, 28U, 30U, 35U, 41U, 47U, 52U, 57U,
      59U, 58U, 57U, 52U, 47U, 41U, 35U, 30U, 28U, 26U, 27U, 30U, 35U, 41U, 47U,
      53U, 57U, 58U, 58U, 57U, 50U, 45U, 38U, 32U, 27U, 25U, 24U, 26U, 29U, 34U,
      40U, 46U, 52U, 56U, 58U, 58U, 56U, 49U, 43U, 35U, 29U, 25U, 23U, 23U, 25U,
      28U, 33U, 39U, 45U, 51U, 55U, 57U, 57U, 56U, 48U, 41U, 34U, 28U, 24U, 22U,
      23U, 24U, 27U, 32U, 37U, 43U, 49U, 53U, 56U, 56U, 56U, 48U, 40U, 32U, 27U,
      24U, 23U, 23U, 24U, 27U, 30U, 35U, 41U, 47U, 51U, 54U, 55U, 56U, 47U, 39U,
      31U, 26U, 24U, 23U, 24U, 25U, 27U, 30U, 34U, 40U, 45U, 50U, 53U, 55U, 55U,
      47U, 38U, 31U, 26U, 24U, 24U, 25U, 26U, 28U, 30U, 34U, 39U, 45U, 49U, 52U,
      54U, 55U, 46U, 38U, 30U, 26U, 24U, 25U, 26U, 28U, 29U, 31U, 35U, 39U, 44U,
      48U, 51U, 53U, 55U, 46U, 37U, 30U, 25U, 24U, 25U, 28U, 30U, 31U, 32U, 35U,
      40U, 44U, 48U, 51U, 53U, 55U, 46U, 38U, 30U, 25U, 24U, 26U, 28U, 31U, 32U,
      33U, 36U, 41U, 45U, 48U, 51U, 53U, 55U, 47U, 38U, 31U, 26U, 24U, 26U, 29U,
      32U, 33U, 34U, 37U, 42U, 46U, 49U, 51U, 53U, 55U, 48U, 40U, 33U, 28U, 25U,
      26U, 29U, 32U, 33U, 35U, 38U, 42U, 47U, 50U, 52U, 54U, 55U, 48U, 41U, 35U,
      30U, 28U, 28U, 30U, 32U, 33U, 35U, 39U, 44U, 47U, 50U, 53U, 54U, 56U, 50U,
      44U, 38U, 34U, 32U, 31U, 32U, 33U, 34U, 36U, 40U, 45U, 49U, 52U, 54U, 55U,
      56U, 51U, 46U, 42U, 39U, 37U, 36U, 36U, 35U, 35U, 37U, 41U, 46U, 50U, 53U,
      55U, 56U, 57U, 52U, 49U, 46U, 44U, 42U, 41U, 40U, 39U, 37U, 39U, 42U, 47U,
      51U, 55U, 57U, 57U, 57U, 54U, 52U, 51U, 49U, 48U, 46U, 45U, 42U, 40U, 40U,
      43U, 48U, 53U, 57U, 59U, 59U, 58U, 55U, 55U, 55U, 54U, 52U, 51U, 48U, 44U,
      41U, 41U, 44U, 49U, 55U, 58U, 60U, 60U, 58U, 57U, 58U, 59U, 58U, 56U, 54U,
      51U, 46U, 42U, 42U, 45U, 50U, 56U, 60U, 61U, 60U, 58U, 58U, 61U, 62U, 61U,
      59U, 56U, 52U, 47U, 43U, 42U, 45U, 51U, 56U, 60U, 62U, 61U, 59U, 59U, 63U,
      64U, 64U, 61U, 57U, 52U, 46U, 42U, 41U, 45U, 50U, 56U, 60U, 61U, 61U, 59U,
      60U, 64U, 66U, 65U, 62U, 57U, 51U, 45U, 41U, 40U, 43U, 49U, 54U, 58U, 61U,
      61U, 59U, 61U, 65U, 67U, 66U, 62U, 57U, 51U, 45U, 40U, 39U, 41U, 46U, 52U,
      56U, 59U, 60U, 59U, 61U, 66U, 67U, 65U, 62U, 56U, 50U, 44U, 39U, 37U, 39U,
      43U, 49U, 54U, 58U, 59U, 59U, 61U, 66U, 66U, 64U, 60U, 55U, 49U, 43U, 38U,
      36U, 37U, 41U, 46U, 52U, 56U, 59U, 58U, 61U, 65U, 65U, 63U, 59U, 53U, 47U,
      41U, 36U, 34U, 35U, 39U, 44U, 50U, 55U, 58U, 58U, 61U, 64U, 64U, 61U, 56U,
      51U, 45U, 40U, 35U, 33U, 34U, 38U, 43U, 49U, 54U, 58U, 58U, 61U, 63U, 62U,
      59U, 54U, 49U, 43U, 38U, 34U, 33U, 34U, 37U, 42U, 48U, 54U, 57U, 58U },
    1.0,
  
    { 0.0, 0.0, 0.0 },
    1.0E+8,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,
    0.0033528131778969143,
    2.0,
    6.378137E+6,
    1.0,
    0,
    2000,
    0,
    8,
    16,
    0,
    18,
    36,
    0,
    1099511628,
    1099511628,
    1099511628,
    1099511628,
    2012227627,
    2012227627,
    1.0F,
    0.0F,
    2.0F,
    1.0F,
    0.0F,
    0.3F,
    0.2F,
    1.0F,
    0.0F,
    1.0F,
    0.0F,
    1.0F,
    0.0F,
    1.0F,
    0.0F,
    1.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    1.0F,
    0.5F,
    2.0F,
  
    { 0.0F, 1.0F },
    0.5F,
    1.0F,
    0.5F,
    1.0F,
    1.0F,
    1.0F,
  
    { 0.0F, 1.0F },
    0.5F,
    1.0F,
    0.5F,
    1.0F,
    1.0F,
    1.0F,
  
    { 0.0F, 1.0F },
    0.5F,
    1.0F,
    0.5F,
    1.0F,
    1.0F,
    1.0F,
  
    { 1.0F, 0.0F, 0.0F, 0.0F },
    -1.0F,
    0.001F,
    0.001F,
    0.001F,
  
    { 1.0F, 0.0F, 0.0F, 0.0F },
    0.002F,
    0.0F,
    4.0F,
    0.002F,
    0.0F,
    2.0F,
    0.002F,
    0.0F,
    1.0F,
    0.0F,
    20.0F,
    0.01F,
    0.003F,
    0.0F,
    1.0F,
    1.0F,
    1.0F,
    0.0F,
    0.002F,
    14.14F,
    0.002F,
    0.0F,
    100.0F,
    0.0F,
    0.0F,
    0.002F,
    0.0F,
    14.14F,
    0.002F,
    100.0F,
    0.0F,
    0.0F,
    0.002F,
    0.2F,
    0.1F,
    1.0F,
    0.0F,
    0.0F,
  
    { 0.0F, 0.0F, -9.8055F },
    -0.2F,
    0.2F,
    1.0F,
    0.1F,
    2.0F,
    1.0F,
    0.0F,
    0.0F,
  
    { 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F,
      0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F,
      0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F,
      0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F,
      0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F,
      0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F, 0.0333333351F },
    0.05F,
    0.5F,
    -0.5F,
    0.0F,
    0.0F,
    0.17453292F,
    1.57079637F,
    -1.57079637F,
    0.001F,
  
    { 0.2F, 0.2F, 0.0F },
    0.002F,
    0.0F,
    -266.532715F,
    35530.5742F,
    0.3F,
    0.2F,
    1.0F,
    0.0F,
    -0.2F,
    9.806F,
    0.5F,
    0.052359879F,
    -0.052359879F,
    3.33333332E-6F,
    -0.00066666666F,
    2.0F,
    1.0F,
    0.05F,
    1.0F,
    0.00174532924F,
    -0.00174532924F,
    0.0F,
    0.002F,
    0.002F,
    0.0F,
    0.87266463F,
    -0.87266463F,
    0.5F,
    2.0F,
    2.0F,
    2.0F,
    2.0F,
    2.0F,
    2.0F,
    -1.0F,
    0.999F,
    -0.999F,
    2.0F,
    0.0F,
    1.0F,
    -1.0F,
    -1.0F,
    0.0F,
    0.0F,
    2.0F,
    -1.0F,
    0.0F,
  
    { 0.0F, 0.0F },
    0.0F,
    0.002F,
  
    { 0.0F, 0.0F },
    0.0F,
    -1.499547F,
    0.353393257F,
    0.0F,
    0.0F,
    0.0F,
    2.121F,
    0.002F,
    0.0F,
    2.0F,
    -1.0F,
    0.0F,
    0.0F,
    0.0F,
    0.002F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    -1.0F,
    0.0F,
    0.002F,
    0.0F,
  
    { 0.0F, 0.0F },
    0.002F,
    0.0F,
    0.0F,
    0.85F,
    0.5F,
    1.0F,
    0.0F,
    0.7F,
    0.25F,
    1.0F,
    0.0F,
    0.85F,
    0.5F,
    1.0F,
    0.0F,
    0.7F,
    0.25F,
    1.0F,
    0.0F,
    1.0F,
    0.0F,
    0.0F,
    0.0F,
    1.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    273.15F,
    0.0F,
    0.0F,
    120000.0F,
    500.0F,
    -29.2692909F,
    0.6F,
    2.0F,
    1.0F,
    0.0F,
    3.0F,
    0.4F,
    1.0F,
    0.0F,
    0.002F,
    0.0F,
    -266.532715F,
    0.002F,
    35530.5742F,
    0.002F,
    0.0F,
    -266.532715F,
    0.002F,
    35530.5742F,
    0.5F,
    1.0F,
    1.0F,
    1.0F,
    1.0F,
    0.0F,
    2.0F,
    1.0F,
    1.0F,
    0.0F,
    1.0F,
    100.0F,
    0.0F,
    -1.0F,
    0.0F,
    0.0F,
    0.0F,
    1.0F,
    1.0F,
    0.0F,
    0.0F,
    1.0F,
    0.0F,
    0.0F,
    0.0F,
    0.0F,
    2199023256U,
    2199023256U,
    0U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    75U,
    75U,
    50U,
    50U,
    1U,
    10U,
    1U,
    75U,
    50U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    1U,
    0U,
    1U,
    4294967295U,
    1U,
    1U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    0U,
    1U,
    1U,
    1U,
    1U,
    1U,
    -18301,
    32768U,
    32768U,
    32768U,
    32768U,
    32768U,
    32768U,
    32768U,
    0U,
    1000U,
    0U,
    125U,
    25U,
    0,
    0,
    1,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    0,
    71,
    128U,
    128U,
    128U,
    128U,
    128U,
    128U,
    128U,
    128U,
    1U,
    0U,
    1U,
    0U,
    1U,
    0U,
    0U,
    0U,
    0U,
    0U,
    1U,
    0U
  };
  
  /* Block signals (default storage) */
  BlockIO_INS_T INS_B;
  
  /* Block states (default storage) */
  D_Work_INS_T INS_DWork;
  
  /* Previous zero-crossings (trigger) states */
  PrevZCSigStates_INS_T INS_PrevZCSigState;
  
  /* External inputs (root inport signals with default storage) */
  ExternalInputs_INS_T INS_U;
  
  /* External outputs (root outports fed by signals with default storage) */
  ExternalOutputs_INS_T INS_Y;
  
  /* Real-time model */
  RT_MODEL_INS_T INS_M_;
  RT_MODEL_INS_T *const INS_M = &INS_M_;
  
  /* Model step function */
  void INS_step(void)
  {
    boolean_T rtb_heading_quality;
    boolean_T rtb_Delay_b;
    uint32_T rtb_DiscreteTimeIntegrator;
    boolean_T rtb_FixPtRelationalOperator_if;
    boolean_T rtb_LogicalOperator3;
    real32_T rtb_DiscreteTimeIntegrator_g;
    real32_T rtb_Product1_g0;
    int32_T rtb_Saturation2;
    uint8_T rtb_Delay_h;
    uint8_T rtb_Compare_cl;
    uint8_T rtb_Delay_fc;
    boolean_T rtb_Compare_cio;
    int32_T rtb_Saturation1;
    real32_T rtb_vel_N;
    real32_T rtb_vel_E;
    real32_T rtb_vel_D;
    real_T rtb_Scalefactor1;
    real_T rtb_Scalefactor2;
    real_T rtb_Scalefactor3;
    real32_T rtb_Multiply_j[3];
    boolean_T rtb_Compare_fz;
    real_T rtb_Gain6;
    real32_T rtb_VectorConcatenate_m[9];
    boolean_T rtb_LogicalOperator_i;
    real32_T rtb_DiscreteTimeIntegrator_df;
    uint16_T rtb_Saturation_p3;
    real32_T rtb_Divide1_m[3];
    boolean_T rtb_FixPtRelationalOperator_l;
    real32_T rtb_ba_O_mDs2[3];
    real32_T rtb_Relay2;
    real32_T rtb_Product3_eb;
    real32_T rtb_TrigonometricFunction1;
    boolean_T rtb_GPS_vaid;
    boolean_T rtb_Compare_le;
    boolean_T rtb_LogicalOperator2_m;
    boolean_T rtb_Compare_e;
    boolean_T rtb_LogicalOperator2_o5;
    boolean_T rtb_Compare_ag;
    boolean_T rtb_Compare_os;
    boolean_T rtb_Compare_f3;
    boolean_T rtb_Compare_b;
    real_T rtb_Min;
    real32_T rtb_Switch_er[6];
    real32_T rtb_Sum2_j;
    real32_T rtb_Sum4_k;
    real32_T rtb_MatrixConcatenate[3];
    real32_T rtb_Add_m;
    real32_T rtb_Sum_g0[6];
    real32_T rtb_Inclination;
    real32_T rtb_uT[3];
    boolean_T rtb_AND_e[3];
    real32_T rtb_Product1_n[3];
    boolean_T rtb_AND_ej[3];
    real32_T rtb_DiscreteTimeIntegrator_h[3];
    real32_T rtb_Switch3[3];
    real32_T rtb_Transpose2[9];
    real32_T rtb_Multiply_j_0[3];
    real32_T tmp[6];
    int32_T i;
    real_T rtb_WGS84_pos_ref_j_idx_0;
    real32_T rtb_vel_NE_0_idx_0;
    real32_T rtb_vel_NE_0_idx_1;
    real32_T rtb_Square_l_idx_0;
    real32_T rtb_Square_l_idx_1;
    real32_T rtb_Delay_m_idx_1;
    real32_T rtb_DiscreteTimeIntegrator_c_id;
    real32_T rtb_DiscreteTimeIntegrator_c__0;
    real_T rtb_WGS84_derivatives_ref_idx_0;
    real_T rtb_WGS84_derivatives_ref_idx_1;
    real32_T rtb_VectorConcatenate_idx_0;
    real32_T rtb_VectorConcatenate_idx_1;
    real32_T rtb_DiscreteTimeIntegrator_c__1;
    real32_T rtb_Delay_m_idx_2;
    uint32_T qY;
    uint32_T qY_0;
    real32_T u0;
    int32_T rtb_Saturation1_0;
  
    /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
    /* Outputs for Atomic SubSystem: '<S3>/Baro_Process' */
    /* Logic: '<S279>/Logical Operator' incorporates:
     *  Constant: '<S288>/Lower Limit'
     *  Constant: '<S288>/Upper Limit'
     *  Constant: '<S289>/Lower Limit'
     *  Constant: '<S289>/Upper Limit'
     *  Inport: '<Root>/Barometer'
     *  Logic: '<S288>/AND'
     *  Logic: '<S289>/AND'
     *  RelationalOperator: '<S288>/Lower Test'
     *  RelationalOperator: '<S288>/Upper Test'
     *  RelationalOperator: '<S289>/Lower Test'
     *  RelationalOperator: '<S289>/Upper Test'
     */
    rtb_heading_quality = ((INS_P.Relative_Height_pressure_min <
      INS_U.Barometer.pressure) && (INS_U.Barometer.pressure <
      INS_P.Relative_Height_pressure_max) && ((INS_P.Relative_Height_temp_min <
      INS_U.Barometer.temperature) && (INS_U.Barometer.temperature <
      INS_P.Relative_Height_temp_max)));
  
    /* RelationalOperator: '<S287>/FixPt Relational Operator' incorporates:
     *  Inport: '<Root>/Barometer'
     *  UnitDelay: '<S287>/Delay Input1'
     */
    rtb_FixPtRelationalOperator_if = (INS_U.Barometer.timestamp !=
      INS_DWork.DelayInput1_DSTATE_g);
  
    /* Sum: '<S279>/Sum1' incorporates:
     *  Constant: '<S281>/Constant'
     *  Constant: '<S286>/Constant'
     *  Delay: '<S279>/Delay'
     *  Logic: '<S279>/Logical Operator1'
     *  Logic: '<S279>/Logical Operator2'
     *  Logic: '<S279>/Logical Operator5'
     *  RelationalOperator: '<S281>/Compare'
     *  RelationalOperator: '<S286>/Compare'
     */
    rtb_Saturation1 = (((rtb_heading_quality && rtb_FixPtRelationalOperator_if) ||
                        (INS_DWork.Delay_DSTATE_f > INS_P.Constant_Value_in)) &&
                       (INS_DWork.Delay_DSTATE_f <
                        INS_P.CompareToConstant_const_pt));
    qY_0 = rtb_Saturation1 + INS_DWork.Delay_DSTATE_f;
    if (qY_0 < (uint32_T)rtb_Saturation1) {
      qY_0 = MAX_uint32_T;
    }
  
    /* DiscreteIntegrator: '<S279>/Discrete-Time Integrator' */
    if (rtb_FixPtRelationalOperator_if) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ev =
        INS_P.DiscreteTimeIntegrator_IC_dv;
    }
  
    /* Logic: '<S279>/Logical Operator3' incorporates:
     *  Constant: '<S282>/Constant'
     *  Constant: '<S284>/Constant'
     *  RelationalOperator: '<S282>/Compare'
     *  RelationalOperator: '<S284>/Compare'
     *  Sum: '<S279>/Sum1'
     */
    rtb_LogicalOperator3 = ((qY_0 >= INS_P.Relative_Height_s_first) && (qY_0 <=
      INS_P.Relative_Height_s_last));
  
    /* Logic: '<S279>/Logical Operator4' incorporates:
     *  Constant: '<S283>/Constant'
     *  Constant: '<S285>/Constant'
     *  DiscreteIntegrator: '<S279>/Discrete-Time Integrator'
     *  RelationalOperator: '<S283>/Compare'
     *  RelationalOperator: '<S285>/Compare'
     *  Sum: '<S279>/Sum1'
     */
    rtb_FixPtRelationalOperator_if = ((qY_0 > INS_P.Relative_Height_s_last) &&
      (INS_DWork.DiscreteTimeIntegrator_DSTAT_ev < INS_P.CompareToConstant4_const)
      && rtb_heading_quality);
  
    /* Outputs for Enabled SubSystem: '<S277>/Reference_and_Derivative' incorporates:
     *  EnablePort: '<S280>/Enable'
     */
    if (rtb_LogicalOperator3) {
      if (!INS_DWork.Reference_and_Derivative_MODE) {
        /* InitializeConditions for Delay: '<S280>/Delay' */
        INS_DWork.Delay_DSTATE_d = INS_P.Delay_InitialCondition_p;
  
        /* InitializeConditions for Delay: '<S280>/Delay1' */
        INS_DWork.Delay1_DSTATE_i = INS_P.Delay1_InitialCondition_k;
  
        /* InitializeConditions for Delay: '<S280>/Delay2' */
        INS_DWork.Delay2_DSTATE = INS_P.Delay2_InitialCondition;
        INS_DWork.Reference_and_Derivative_MODE = true;
      }
  
      /* Sum: '<S280>/Sum2' incorporates:
       *  Delay: '<S280>/Delay'
       *  Inport: '<Root>/Barometer'
       */
      rtb_Sum2_j = INS_U.Barometer.pressure + INS_DWork.Delay_DSTATE_d;
  
      /* Sum: '<S280>/Sum1' incorporates:
       *  Constant: '<S280>/Constant1'
       *  Delay: '<S280>/Delay1'
       */
      qY = INS_P.Constant1_Value_f + INS_DWork.Delay1_DSTATE_i;
      if (qY < INS_P.Constant1_Value_f) {
        qY = MAX_uint32_T;
      }
  
      /* Saturate: '<S280>/Saturation1' incorporates:
       *  Sum: '<S280>/Sum1'
       */
      if (qY > INS_P.Saturation1_UpperSat_n) {
        rtb_DiscreteTimeIntegrator = INS_P.Saturation1_UpperSat_n;
      } else if (qY < INS_P.Saturation1_LowerSat_m) {
        rtb_DiscreteTimeIntegrator = INS_P.Saturation1_LowerSat_m;
      } else {
        rtb_DiscreteTimeIntegrator = qY;
      }
  
      /* End of Saturate: '<S280>/Saturation1' */
  
      /* Product: '<S280>/Divide' */
      INS_B.Divide = rtb_Sum2_j / (real32_T)rtb_DiscreteTimeIntegrator;
  
      /* Sum: '<S280>/Sum4' incorporates:
       *  Constant: '<S280>/Constant2'
       *  Delay: '<S280>/Delay2'
       *  Inport: '<Root>/Barometer'
       *  Sum: '<S280>/Sum3'
       */
      rtb_Sum4_k = (INS_P.Constant2_Value_p + INS_U.Barometer.temperature) +
        INS_DWork.Delay2_DSTATE;
  
      /* Saturate: '<S280>/Saturation' */
      if (INS_B.Divide > INS_P.Saturation_UpperSat_m) {
        rtb_Delay_m_idx_2 = INS_P.Saturation_UpperSat_m;
      } else if (INS_B.Divide < INS_P.Saturation_LowerSat_h) {
        rtb_Delay_m_idx_2 = INS_P.Saturation_LowerSat_h;
      } else {
        rtb_Delay_m_idx_2 = INS_B.Divide;
      }
  
      /* End of Saturate: '<S280>/Saturation' */
  
      /* Gain: '<S280>/gain' incorporates:
       *  Product: '<S280>/Divide1'
       *  Product: '<S280>/Product'
       */
      INS_B.gain = 1.0F / rtb_Delay_m_idx_2 * (rtb_Sum4_k / (real32_T)
        rtb_DiscreteTimeIntegrator) * INS_P.gain_Gain;
  
      /* Update for Delay: '<S280>/Delay' */
      INS_DWork.Delay_DSTATE_d = rtb_Sum2_j;
  
      /* Update for Delay: '<S280>/Delay1' incorporates:
       *  Sum: '<S280>/Sum1'
       */
      INS_DWork.Delay1_DSTATE_i = qY;
  
      /* Update for Delay: '<S280>/Delay2' */
      INS_DWork.Delay2_DSTATE = rtb_Sum4_k;
    } else {
      if (INS_DWork.Reference_and_Derivative_MODE) {
        INS_DWork.Reference_and_Derivative_MODE = false;
      }
    }
  
    /* End of Outputs for SubSystem: '<S277>/Reference_and_Derivative' */
  
    /* Switch: '<S277>/Switch' incorporates:
     *  Inport: '<Root>/Barometer'
     *  Logic: '<S277>/Logical Operator1'
     *  Logic: '<S277>/Logical Operator2'
     */
    if (rtb_LogicalOperator3 || (!rtb_FixPtRelationalOperator_if)) {
      rtb_Sum2_j = INS_U.Barometer.pressure;
    } else {
      rtb_Sum2_j = INS_B.Divide;
    }
  
    /* End of Switch: '<S277>/Switch' */
  
    /* Product: '<S277>/Product1' incorporates:
     *  Inport: '<Root>/Barometer'
     *  Sum: '<S277>/Sum2'
     */
    rtb_Product1_g0 = (INS_U.Barometer.pressure - rtb_Sum2_j) * INS_B.gain;
  
    /* Update for Delay: '<S279>/Delay' incorporates:
     *  Sum: '<S279>/Sum1'
     */
    INS_DWork.Delay_DSTATE_f = qY_0;
  
    /* Update for UnitDelay: '<S287>/Delay Input1' incorporates:
     *  Inport: '<Root>/Barometer'
     */
    INS_DWork.DelayInput1_DSTATE_g = INS_U.Barometer.timestamp;
  
    /* Update for DiscreteIntegrator: '<S279>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S279>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTAT_ev +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ev = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ev = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S279>/Discrete-Time Integrator' */
    /* End of Outputs for SubSystem: '<S3>/Baro_Process' */
  
    /* Outputs for Atomic SubSystem: '<S3>/GPS_Process' */
    /* RelationalOperator: '<S305>/Compare' incorporates:
     *  Constant: '<S305>/Constant'
     *  UnitDelay: '<S304>/Unit Delay'
     */
    rtb_heading_quality = (INS_DWork.UnitDelay_DSTATE >=
      INS_P.CompareToConstant3_const);
  
    /* RelationalOperator: '<S302>/Compare' incorporates:
     *  Constant: '<S302>/Constant'
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_Delay_h = (uint8_T)(INS_U.GPS_uBlox.fixType ==
      INS_P.CompareToConstant1_const_n);
  
    /* DiscreteIntegrator: '<S300>/Discrete-Time Integrator' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     *  RelationalOperator: '<S306>/FixPt Relational Operator'
     *  UnitDelay: '<S306>/Delay Input1'
     */
    if (INS_U.GPS_uBlox.timestamp != INS_DWork.DelayInput1_DSTATE_i) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_i =
        INS_P.DiscreteTimeIntegrator_IC_p;
    }
  
    /* RelationalOperator: '<S303>/Compare' incorporates:
     *  Constant: '<S303>/Constant'
     *  DiscreteIntegrator: '<S300>/Discrete-Time Integrator'
     */
    rtb_Compare_cl = (uint8_T)(INS_DWork.DiscreteTimeIntegrator_DSTATE_i <
      INS_P.CompareToConstant2_const);
  
    /* Sum: '<S297>/Sum1' incorporates:
     *  Logic: '<S297>/Logical Operator1'
     */
    rtb_Delay_fc = (uint8_T)((uint32_T)(rtb_heading_quality && (rtb_Delay_h != 0) &&
      (rtb_Compare_cl != 0)) + rtb_heading_quality);
  
    /* RelationalOperator: '<S290>/Compare' incorporates:
     *  Constant: '<S290>/Constant'
     */
    rtb_LogicalOperator3 = (rtb_Delay_fc > INS_P.CompareToConstant_const_i);
  
    /* RelationalOperator: '<S291>/Compare' incorporates:
     *  Constant: '<S291>/Constant'
     */
    rtb_Compare_cio = (rtb_Delay_fc < INS_P.CompareToConstant1_const_l);
  
    /* RelationalOperator: '<S295>/Compare' incorporates:
     *  Constant: '<S295>/Constant'
     */
    rtb_heading_quality = (rtb_Delay_fc > INS_P.CompareToConstant2_const_bv);
  
    /* RelationalOperator: '<S296>/Compare' incorporates:
     *  Constant: '<S296>/Constant'
     *  DiscreteIntegrator: '<S300>/Discrete-Time Integrator'
     */
    rtb_Delay_b = (INS_DWork.DiscreteTimeIntegrator_DSTATE_i <
                   INS_P.CompareToConstant4_const_e);
  
    /* Gain: '<S299>/Gain3' incorporates:
     *  Constant: '<S299>/Constant7'
     *  Gain: '<S299>/Gain5'
     *  Inport: '<Root>/GPS_uBlox'
     *  Sum: '<S299>/Sum'
     */
    u0 = (INS_P.Constant7_Value - (real32_T)INS_P.Gain5_Gain_a * 4.54747351E-13F *
          (real32_T)INS_U.GPS_uBlox.sAcc) * INS_P.Gain3_Gain_lx;
  
    /* Product: '<S292>/Double' incorporates:
     *  Saturate: '<S292>/Saturation'
     */
    rtb_Sum2_j = rtb_heading_quality ? rtb_Delay_b ? u0 >
      INS_P.Saturation_UpperSat_l ? INS_P.Saturation_UpperSat_l : u0 <
      INS_P.Saturation_LowerSat_ku ? INS_P.Saturation_LowerSat_ku : u0 : 0.0F :
      0.0F;
  
    /* Gain: '<S298>/Gain2' incorporates:
     *  Constant: '<S298>/Constant1'
     *  Gain: '<S298>/Gain1'
     *  Inport: '<Root>/GPS_uBlox'
     *  Sum: '<S298>/Sum1'
     */
    u0 = (INS_P.Constant1_Value_g5 - (real32_T)INS_P.Gain1_Gain_p *
          4.54747351E-13F * (real32_T)INS_U.GPS_uBlox.hAcc) * INS_P.Gain2_Gain_p;
  
    /* Product: '<S292>/Double1' incorporates:
     *  Saturate: '<S292>/Saturation1'
     */
    rtb_Sum4_k = rtb_heading_quality ? rtb_Delay_b ? u0 >
      INS_P.Saturation1_UpperSat_m ? INS_P.Saturation1_UpperSat_m : u0 <
      INS_P.Saturation1_LowerSat_d ? INS_P.Saturation1_LowerSat_d : u0 : 0.0F :
      0.0F;
  
    /* Gain: '<S293>/Gain' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_vel_N = (real32_T)INS_P.Gain_Gain * 9.09494702E-13F * (real32_T)
      INS_U.GPS_uBlox.velN;
  
    /* Gain: '<S293>/Gain1' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_vel_E = (real32_T)INS_P.Gain1_Gain * 9.09494702E-13F * (real32_T)
      INS_U.GPS_uBlox.velE;
  
    /* Gain: '<S293>/Gain2' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_vel_D = (real32_T)INS_P.Gain2_Gain * 9.09494702E-13F * (real32_T)
      INS_U.GPS_uBlox.velD;
  
    /* Gain: '<S293>/Scalefactor1' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_Scalefactor1 = (real_T)INS_P.Scalefactor1_Gain_d * 8.6736173798840355E-19 *
      (real_T)INS_U.GPS_uBlox.lat;
  
    /* Gain: '<S293>/Scalefactor2' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_Scalefactor2 = (real_T)INS_P.Scalefactor2_Gain * 8.6736173798840355E-19 *
      (real_T)INS_U.GPS_uBlox.lon;
  
    /* Gain: '<S293>/Scalefactor3' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    rtb_Scalefactor3 = (real_T)INS_P.Scalefactor3_Gain * 9.0949470177292824E-13 *
      (real_T)INS_U.GPS_uBlox.height;
  
    /* Outputs for Triggered SubSystem: '<S271>/WGS84_Model' incorporates:
     *  TriggerPort: '<S294>/Trigger'
     */
    if (rtb_LogicalOperator3 && (INS_PrevZCSigState.WGS84_Model_Trig_ZCE !=
         POS_ZCSIG)) {
      /* Gain: '<S294>/Scalefactor' */
      INS_B.Scalefactor = INS_P.Scalefactor_Gain * rtb_Scalefactor2;
  
      /* SignalConversion: '<S294>/OutportBufferFordz_dh' incorporates:
       *  Constant: '<S307>/constant1'
       */
      INS_B.OutportBufferFordz_dh = INS_P.constant1_Value;
  
      /* Gain: '<S294>/Scalefactor1' */
      INS_B.Scalefactor1 = INS_P.Scalefactor1_Gain * rtb_Scalefactor1;
  
      /* Trigonometry: '<S307>/Trigonometric Function1' incorporates:
       *  Trigonometry: '<S307>/Trigonometric Function2'
       */
      rtb_WGS84_pos_ref_j_idx_0 = cos(INS_B.Scalefactor1);
  
      /* Product: '<S307>/Product1' incorporates:
       *  Constant: '<S307>/constant'
       *  Constant: '<S307>/f'
       *  Sum: '<S307>/Subtract1'
       */
      rtb_Gain6 = (INS_P.constant_Value - INS_P.f_Value) * INS_P.f_Value;
  
      /* Trigonometry: '<S307>/Trigonometric Function' */
      rtb_Min = sin(INS_B.Scalefactor1);
  
      /* Sum: '<S307>/Subtract' incorporates:
       *  Constant: '<S307>/a1'
       *  Math: '<S307>/u^2'
       *  Product: '<S307>/Product'
       */
      rtb_Min = INS_P.a1_Value - rtb_Min * rtb_Min * rtb_Gain6;
  
      /* Math: '<S307>/sqrt'
       *
       * About '<S307>/sqrt':
       *  Operator: sqrt
       */
      if (rtb_Min < 0.0) {
        rtb_Min = -sqrt(fabs(rtb_Min));
      } else {
        rtb_Min = sqrt(rtb_Min);
      }
  
      /* End of Math: '<S307>/sqrt' */
  
      /* Product: '<S307>/Divide' incorporates:
       *  Constant: '<S307>/a'
       */
      rtb_Min = INS_P.a_Value / rtb_Min;
  
      /* Sum: '<S307>/Add' incorporates:
       *  Constant: '<S307>/constant2'
       *  Math: '<S307>/u^1'
       *  Product: '<S307>/Product2'
       *  Product: '<S307>/Product3'
       *  Sum: '<S307>/Subtract2'
       *  Trigonometry: '<S307>/Trigonometric Function1'
       */
      INS_B.Add = (INS_P.constant2_Value - rtb_WGS84_pos_ref_j_idx_0 *
                   rtb_WGS84_pos_ref_j_idx_0 * rtb_Gain6) * rtb_Min +
        rtb_Scalefactor3;
  
      /* Product: '<S307>/Product4' incorporates:
       *  Sum: '<S307>/Add1'
       */
      INS_B.Product4 = (rtb_Scalefactor3 + rtb_Min) * rtb_WGS84_pos_ref_j_idx_0;
    }
  
    INS_PrevZCSigState.WGS84_Model_Trig_ZCE = rtb_LogicalOperator3;
  
    /* End of Outputs for SubSystem: '<S271>/WGS84_Model' */
  
    /* Sum: '<S304>/Sum' incorporates:
     *  Constant: '<S301>/Constant'
     *  Inport: '<Root>/GPS_uBlox'
     *  Logic: '<S297>/Logical Operator'
     *  RelationalOperator: '<S301>/Compare'
     *  UnitDelay: '<S304>/Unit Delay'
     */
    rtb_Saturation1 = ((INS_U.GPS_uBlox.hAcc < INS_P.CompareToConstant_const_d) &&
                       (rtb_Delay_h != 0) && (rtb_Compare_cl != 0)) +
      INS_DWork.UnitDelay_DSTATE;
  
    /* Saturate: '<S304>/Saturation' */
    if (rtb_Saturation1 > INS_P.Saturation_UpperSat_a) {
      /* Update for UnitDelay: '<S304>/Unit Delay' */
      INS_DWork.UnitDelay_DSTATE = INS_P.Saturation_UpperSat_a;
    } else if (rtb_Saturation1 < INS_P.Saturation_LowerSat) {
      /* Update for UnitDelay: '<S304>/Unit Delay' */
      INS_DWork.UnitDelay_DSTATE = INS_P.Saturation_LowerSat;
    } else {
      /* Update for UnitDelay: '<S304>/Unit Delay' */
      INS_DWork.UnitDelay_DSTATE = rtb_Saturation1;
    }
  
    /* End of Saturate: '<S304>/Saturation' */
  
    /* Update for UnitDelay: '<S306>/Delay Input1' incorporates:
     *  Inport: '<Root>/GPS_uBlox'
     */
    INS_DWork.DelayInput1_DSTATE_i = INS_U.GPS_uBlox.timestamp;
  
    /* Update for DiscreteIntegrator: '<S300>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S300>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTATE_i +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_i = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_i = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S300>/Discrete-Time Integrator' */
    /* End of Outputs for SubSystem: '<S3>/GPS_Process' */
  
    /* Outputs for Atomic SubSystem: '<S3>/IMU_Process' */
    /* Switch: '<S319>/Switch' incorporates:
     *  Constant: '<S319>/Constant'
     *  Constant: '<S319>/Gyr_RotMatrix'
     *  Inport: '<Root>/IMU1'
     *  Product: '<S319>/Product1'
     *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn1'
     *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn2'
     *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn3'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch_Threshold_c) {
      rtb_uT[0] = INS_U.IMU1.gyr_x;
      rtb_uT[1] = INS_U.IMU1.gyr_y;
      rtb_uT[2] = INS_U.IMU1.gyr_z;
    } else {
      /* Sum: '<S319>/Subtract' incorporates:
       *  Constant: '<S319>/Gyro_Bias'
       *  Inport: '<Root>/IMU1'
       *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn1'
       *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn2'
       *  SignalConversion: '<S309>/ConcatBufferAtVector ConcatenateIn3'
       */
      rtb_VectorConcatenate_idx_0 = INS_U.IMU1.gyr_x - INS_PARAM.GyroBias[0];
      rtb_VectorConcatenate_idx_1 = INS_U.IMU1.gyr_y - INS_PARAM.GyroBias[1];
      rtb_Product3_eb = INS_U.IMU1.gyr_z - INS_PARAM.GyroBias[2];
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
        rtb_uT[rtb_Saturation1] = INS_PARAM.GyroRotMat[rtb_Saturation1 + 6] *
          rtb_Product3_eb + (INS_PARAM.GyroRotMat[rtb_Saturation1 + 3] *
                             rtb_VectorConcatenate_idx_1 +
                             INS_PARAM.GyroRotMat[rtb_Saturation1] *
                             rtb_VectorConcatenate_idx_0);
      }
    }
  
    /* End of Switch: '<S319>/Switch' */
  
    /* Switch: '<S318>/Switch' incorporates:
     *  Constant: '<S318>/Acc_RotMatrix'
     *  Constant: '<S318>/Constant'
     *  Inport: '<Root>/IMU1'
     *  Product: '<S318>/Product5'
     *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In1'
     *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In2'
     *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In3'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch_Threshold_k) {
      rtb_Product1_n[0] = INS_U.IMU1.acc_x;
      rtb_Product1_n[1] = INS_U.IMU1.acc_y;
      rtb_Product1_n[2] = INS_U.IMU1.acc_z;
    } else {
      /* Sum: '<S318>/Subtract1' incorporates:
       *  Constant: '<S318>/Acc_Bias'
       *  Inport: '<Root>/IMU1'
       *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In1'
       *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In2'
       *  SignalConversion: '<S309>/ConcatBufferAtVector Concatenate1In3'
       */
      rtb_VectorConcatenate_idx_0 = INS_U.IMU1.acc_x - INS_PARAM.AccBias[0];
      rtb_VectorConcatenate_idx_1 = INS_U.IMU1.acc_y - INS_PARAM.AccBias[1];
      rtb_Product3_eb = INS_U.IMU1.acc_z - INS_PARAM.AccBias[2];
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
        rtb_Product1_n[rtb_Saturation1] = INS_PARAM.AccRotMat[rtb_Saturation1 + 6]
          * rtb_Product3_eb + (INS_PARAM.AccRotMat[rtb_Saturation1 + 3] *
          rtb_VectorConcatenate_idx_1 + INS_PARAM.AccRotMat[rtb_Saturation1] *
          rtb_VectorConcatenate_idx_0);
      }
    }
  
    /* End of Switch: '<S318>/Switch' */
  
    /* DiscreteIntegrator: '<S310>/Discrete-Time Integrator' incorporates:
     *  Inport: '<Root>/IMU1'
     *  RelationalOperator: '<S321>/FixPt Relational Operator'
     *  UnitDelay: '<S321>/Delay Input1'
     */
    if (INS_U.IMU1.timestamp != INS_DWork.DelayInput1_DSTATE_h) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_lp =
        INS_P.DiscreteTimeIntegrator_IC_b;
    }
  
    /* Logic: '<S310>/Logical Operator4' incorporates:
     *  Constant: '<S320>/Constant'
     *  Constant: '<S322>/Lower Limit'
     *  Constant: '<S322>/Upper Limit'
     *  Constant: '<S323>/Lower Limit'
     *  Constant: '<S323>/Upper Limit'
     *  DiscreteIntegrator: '<S310>/Discrete-Time Integrator'
     *  Logic: '<S310>/Logical Operator'
     *  Logic: '<S310>/Logical Operator1'
     *  Logic: '<S310>/Logical Operator2'
     *  Logic: '<S322>/AND'
     *  Logic: '<S323>/AND'
     *  RelationalOperator: '<S320>/Compare'
     *  RelationalOperator: '<S322>/Lower Test'
     *  RelationalOperator: '<S322>/Upper Test'
     *  RelationalOperator: '<S323>/Lower Test'
     *  RelationalOperator: '<S323>/Upper Test'
     */
    rtb_heading_quality = ((INS_P.IMU1_Valid_Check_gyr_min < rtb_uT[0]) &&
      (rtb_uT[0] < INS_P.IMU1_Valid_Check_gyr_max) &&
      ((INS_P.IMU1_Valid_Check_gyr_min < rtb_uT[1]) && (rtb_uT[1] <
      INS_P.IMU1_Valid_Check_gyr_max)) && ((INS_P.IMU1_Valid_Check_gyr_min <
      rtb_uT[2]) && (rtb_uT[2] < INS_P.IMU1_Valid_Check_gyr_max)) &&
      ((INS_P.IMU1_Valid_Check_acc_min < rtb_Product1_n[0]) && (rtb_Product1_n[0] <
      INS_P.IMU1_Valid_Check_acc_max) && ((INS_P.IMU1_Valid_Check_acc_min <
      rtb_Product1_n[1]) && (rtb_Product1_n[1] < INS_P.IMU1_Valid_Check_acc_max)) &&
       ((INS_P.IMU1_Valid_Check_acc_min < rtb_Product1_n[2]) && (rtb_Product1_n[2]
      < INS_P.IMU1_Valid_Check_acc_max))) &&
      (INS_DWork.DiscreteTimeIntegrator_DSTAT_lp < INS_P.IMU1_Valid_Check_timeout));
  
    /* Sum: '<S325>/Subtract' incorporates:
     *  Constant: '<S325>/Gyro_Bias'
     *  Inport: '<Root>/IMU2'
     *  SignalConversion: '<S311>/ConcatBufferAtVector ConcatenateIn1'
     *  SignalConversion: '<S311>/ConcatBufferAtVector ConcatenateIn2'
     *  SignalConversion: '<S311>/ConcatBufferAtVector ConcatenateIn3'
     */
    rtb_VectorConcatenate_idx_0 = INS_U.IMU2.gyr_x - INS_PARAM.GyroBias2[0];
    rtb_VectorConcatenate_idx_1 = INS_U.IMU2.gyr_y - INS_PARAM.GyroBias2[1];
    rtb_Product3_eb = INS_U.IMU2.gyr_z - INS_PARAM.GyroBias2[2];
  
    /* SignalConversion: '<S311>/ConcatBufferAtVector Concatenate1In1' incorporates:
     *  Inport: '<Root>/IMU2'
     */
    rtb_Multiply_j[0] = INS_U.IMU2.acc_x;
  
    /* SignalConversion: '<S311>/ConcatBufferAtVector Concatenate1In2' incorporates:
     *  Inport: '<Root>/IMU2'
     */
    rtb_Multiply_j[1] = INS_U.IMU2.acc_y;
  
    /* SignalConversion: '<S311>/ConcatBufferAtVector Concatenate1In3' incorporates:
     *  Inport: '<Root>/IMU2'
     */
    rtb_Multiply_j[2] = INS_U.IMU2.acc_z;
    for (i = 0; i < 3; i++) {
      /* Product: '<S325>/Product1' incorporates:
       *  Constant: '<S325>/Gyr_RotMatrix'
       */
      u0 = INS_PARAM.GyroRotMat2[i + 6] * rtb_Product3_eb +
        (INS_PARAM.GyroRotMat2[i + 3] * rtb_VectorConcatenate_idx_1 +
         INS_PARAM.GyroRotMat2[i] * rtb_VectorConcatenate_idx_0);
  
      /* Logic: '<S329>/AND' incorporates:
       *  Constant: '<S329>/Lower Limit'
       *  Constant: '<S329>/Upper Limit'
       *  RelationalOperator: '<S329>/Lower Test'
       *  RelationalOperator: '<S329>/Upper Test'
       */
      rtb_AND_e[i] = ((INS_P.IMU2_Valid_Check_gyr_min < u0) && (u0 <
        INS_P.IMU2_Valid_Check_gyr_max));
  
      /* Sum: '<S324>/Subtract1' incorporates:
       *  Constant: '<S324>/Acc_Bias'
       */
      rtb_Multiply_j_0[i] = rtb_Multiply_j[i] - INS_PARAM.AccBias2[i];
  
      /* Product: '<S325>/Product1' incorporates:
       *  Constant: '<S325>/Gyr_RotMatrix'
       */
      rtb_Divide1_m[i] = u0;
    }
  
    for (i = 0; i < 3; i++) {
      /* Product: '<S324>/Product5' incorporates:
       *  Constant: '<S324>/Acc_RotMatrix'
       */
      rtb_VectorConcatenate_idx_0 = INS_PARAM.AccRotMat2[i + 6] *
        rtb_Multiply_j_0[2] + (INS_PARAM.AccRotMat2[i + 3] * rtb_Multiply_j_0[1] +
        INS_PARAM.AccRotMat2[i] * rtb_Multiply_j_0[0]);
  
      /* Logic: '<S328>/AND' incorporates:
       *  Constant: '<S328>/Lower Limit'
       *  Constant: '<S328>/Upper Limit'
       *  RelationalOperator: '<S328>/Lower Test'
       *  RelationalOperator: '<S328>/Upper Test'
       */
      rtb_AND_ej[i] = ((INS_P.IMU2_Valid_Check_acc_min <
                        rtb_VectorConcatenate_idx_0) &&
                       (rtb_VectorConcatenate_idx_0 <
                        INS_P.IMU2_Valid_Check_acc_max));
  
      /* Product: '<S324>/Product5' incorporates:
       *  Constant: '<S324>/Acc_RotMatrix'
       */
      rtb_ba_O_mDs2[i] = rtb_VectorConcatenate_idx_0;
    }
  
    /* DiscreteIntegrator: '<S312>/Discrete-Time Integrator' incorporates:
     *  Inport: '<Root>/IMU2'
     *  RelationalOperator: '<S327>/FixPt Relational Operator'
     *  UnitDelay: '<S327>/Delay Input1'
     */
    if (INS_U.IMU2.timestamp != INS_DWork.DelayInput1_DSTATE_ht) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_d =
        INS_P.DiscreteTimeIntegrator_IC_pw;
    }
  
    /* Sum: '<S315>/Sum' incorporates:
     *  Constant: '<S326>/Constant'
     *  DiscreteIntegrator: '<S312>/Discrete-Time Integrator'
     *  Gain: '<S315>/Gain'
     *  Logic: '<S312>/Logical Operator'
     *  Logic: '<S312>/Logical Operator1'
     *  Logic: '<S312>/Logical Operator2'
     *  Logic: '<S312>/Logical Operator4'
     *  RelationalOperator: '<S326>/Compare'
     */
    rtb_Delay_h = (uint8_T)((uint32_T)(rtb_AND_e[0] && rtb_AND_e[1] && rtb_AND_e[2]
      && (rtb_AND_ej[0] && rtb_AND_ej[1] && rtb_AND_ej[2]) &&
      (INS_DWork.DiscreteTimeIntegrator_DSTATE_d < INS_P.IMU2_Valid_Check_timeout)
      ? (int32_T)((uint32_T)INS_P.Gain_Gain_od >> 6) : 0) + rtb_heading_quality);
  
    /* RelationalOperator: '<S313>/Compare' incorporates:
     *  Constant: '<S313>/Constant'
     */
    rtb_Compare_fz = (rtb_Delay_h > INS_P.Constant_Value_jn);
  
    /* DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_h[0] = INS_DWork.DiscreteTimeIntegrator_DSTATE[0];
  
    /* Switch: '<S314>/Switch' */
    if (!rtb_heading_quality) {
      rtb_uT[0] = rtb_Divide1_m[0];
      rtb_uT[1] = rtb_Divide1_m[1];
    }
  
    /* DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_h[1] = INS_DWork.DiscreteTimeIntegrator_DSTATE[1];
  
    /* Switch: '<S314>/Switch' */
    rtb_DiscreteTimeIntegrator_g = rtb_uT[2];
  
    /* DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_h[2] = INS_DWork.DiscreteTimeIntegrator_DSTATE[2];
  
    /* Switch: '<S314>/Switch' */
    if (!rtb_heading_quality) {
      rtb_DiscreteTimeIntegrator_g = rtb_Divide1_m[2];
    }
  
    /* DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
    if (INS_DWork.DiscreteTimeIntegrator1_IC_LOAD != 0) {
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[0] = rtb_uT[0];
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[1] = rtb_uT[1];
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[2] = rtb_DiscreteTimeIntegrator_g;
    }
  
    /* DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_c_id = INS_DWork.DiscreteTimeIntegrator_DSTATE_c[0];
  
    /* Switch: '<S314>/Switch1' */
    if (!rtb_heading_quality) {
      rtb_Product1_n[0] = rtb_ba_O_mDs2[0];
      rtb_Product1_n[1] = rtb_ba_O_mDs2[1];
    }
  
    /* DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_c__0 = INS_DWork.DiscreteTimeIntegrator_DSTATE_c[1];
  
    /* Switch: '<S314>/Switch1' */
    rtb_Relay2 = rtb_Product1_n[2];
  
    /* DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator_c__1 = INS_DWork.DiscreteTimeIntegrator_DSTATE_c[2];
  
    /* Switch: '<S314>/Switch1' */
    if (!rtb_heading_quality) {
      rtb_Relay2 = rtb_ba_O_mDs2[2];
    }
  
    /* DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
    if (INS_DWork.DiscreteTimeIntegrator1_IC_LO_a != 0) {
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[0] = rtb_Product1_n[0];
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[1] = rtb_Product1_n[1];
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[2] = rtb_Relay2;
    }
  
    /* Update for UnitDelay: '<S321>/Delay Input1' incorporates:
     *  Inport: '<Root>/IMU1'
     */
    INS_DWork.DelayInput1_DSTATE_h = INS_U.IMU1.timestamp;
  
    /* Update for DiscreteIntegrator: '<S310>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S310>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTAT_lp +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_lp = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_lp = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S310>/Discrete-Time Integrator' */
  
    /* Update for UnitDelay: '<S327>/Delay Input1' incorporates:
     *  Inport: '<Root>/IMU2'
     */
    INS_DWork.DelayInput1_DSTATE_ht = INS_U.IMU2.timestamp;
  
    /* Update for DiscreteIntegrator: '<S312>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S312>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTATE_d +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_d = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_d = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S312>/Discrete-Time Integrator' */
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_IC_LOAD = 0U;
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_IC_LO_a = 0U;
  
    /* Switch: '<S314>/Switch2' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch2_Threshold) {
      rtb_VectorConcatenate_idx_0 = rtb_uT[0];
    } else {
      rtb_VectorConcatenate_idx_0 = INS_DWork.DiscreteTimeIntegrator1_DSTATE[0];
    }
  
    /* Switch: '<S314>/Switch3' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch3_Threshold) {
      rtb_Switch3[0] = rtb_Product1_n[0];
    } else {
      rtb_Switch3[0] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[0];
    }
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     *  Gain: '<S316>/Gain'
     *  Gain: '<S316>/Gain1'
     *  Sum: '<S316>/Add'
     *  Sum: '<S316>/Sum'
     *  Switch: '<S314>/Switch2'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE[0] += ((rtb_uT[0] -
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[0]) * INS_P.Gain_Gain_mz +
      INS_P.Gain1_Gain_kl * INS_DWork.DiscreteTimeIntegrator_DSTATE[0]) *
      INS_P.DiscreteTimeIntegrator_gainv_kc;
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTATE[0] +=
      INS_P.DiscreteTimeIntegrator1_gainv_i * rtb_DiscreteTimeIntegrator_h[0];
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     *  Gain: '<S317>/Gain'
     *  Gain: '<S317>/Gain1'
     *  Sum: '<S317>/Add'
     *  Sum: '<S317>/Sum'
     *  Switch: '<S314>/Switch3'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_c[0] += ((rtb_Product1_n[0] -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[0]) * INS_P.Gain_Gain_m2 +
      INS_P.Gain1_Gain_e1 * INS_DWork.DiscreteTimeIntegrator_DSTATE_c[0]) *
      INS_P.DiscreteTimeIntegrator_gainv_mk;
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[0] +=
      INS_P.DiscreteTimeIntegrator1_gainv_p * rtb_DiscreteTimeIntegrator_c_id;
  
    /* Switch: '<S314>/Switch2' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch2_Threshold) {
      rtb_VectorConcatenate_idx_1 = rtb_uT[1];
    } else {
      rtb_VectorConcatenate_idx_1 = INS_DWork.DiscreteTimeIntegrator1_DSTATE[1];
    }
  
    /* Switch: '<S314>/Switch3' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch3_Threshold) {
      rtb_Switch3[1] = rtb_Product1_n[1];
    } else {
      rtb_Switch3[1] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[1];
    }
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     *  Gain: '<S316>/Gain'
     *  Gain: '<S316>/Gain1'
     *  Sum: '<S316>/Add'
     *  Sum: '<S316>/Sum'
     *  Switch: '<S314>/Switch2'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE[1] += ((rtb_uT[1] -
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[1]) * INS_P.Gain_Gain_mz +
      INS_P.Gain1_Gain_kl * INS_DWork.DiscreteTimeIntegrator_DSTATE[1]) *
      INS_P.DiscreteTimeIntegrator_gainv_kc;
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTATE[1] +=
      INS_P.DiscreteTimeIntegrator1_gainv_i * rtb_DiscreteTimeIntegrator_h[1];
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     *  Gain: '<S317>/Gain'
     *  Gain: '<S317>/Gain1'
     *  Sum: '<S317>/Add'
     *  Sum: '<S317>/Sum'
     *  Switch: '<S314>/Switch3'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_c[1] += ((rtb_Product1_n[1] -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[1]) * INS_P.Gain_Gain_m2 +
      INS_P.Gain1_Gain_e1 * INS_DWork.DiscreteTimeIntegrator_DSTATE_c[1]) *
      INS_P.DiscreteTimeIntegrator_gainv_mk;
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[1] +=
      INS_P.DiscreteTimeIntegrator1_gainv_p * rtb_DiscreteTimeIntegrator_c__0;
  
    /* Switch: '<S314>/Switch2' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch2_Threshold) {
      rtb_Product3_eb = rtb_DiscreteTimeIntegrator_g;
    } else {
      rtb_Product3_eb = INS_DWork.DiscreteTimeIntegrator1_DSTATE[2];
    }
  
    /* Switch: '<S314>/Switch3' incorporates:
     *  Constant: '<S314>/Constant'
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     */
    if (INS_PARAM.USE_EXTERN_FILTER > INS_P.Switch3_Threshold) {
      rtb_Switch3[2] = rtb_Relay2;
    } else {
      rtb_Switch3[2] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[2];
    }
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S316>/Discrete-Time Integrator1'
     *  Gain: '<S316>/Gain'
     *  Gain: '<S316>/Gain1'
     *  Sum: '<S316>/Add'
     *  Sum: '<S316>/Sum'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE[2] += ((rtb_DiscreteTimeIntegrator_g -
      INS_DWork.DiscreteTimeIntegrator1_DSTATE[2]) * INS_P.Gain_Gain_mz +
      INS_P.Gain1_Gain_kl * INS_DWork.DiscreteTimeIntegrator_DSTATE[2]) *
      INS_P.DiscreteTimeIntegrator_gainv_kc;
  
    /* Update for DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTATE[2] +=
      INS_P.DiscreteTimeIntegrator1_gainv_i * rtb_DiscreteTimeIntegrator_h[2];
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' incorporates:
     *  DiscreteIntegrator: '<S317>/Discrete-Time Integrator1'
     *  Gain: '<S317>/Gain'
     *  Gain: '<S317>/Gain1'
     *  Sum: '<S317>/Add'
     *  Sum: '<S317>/Sum'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_c[2] += ((rtb_Relay2 -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[2]) * INS_P.Gain_Gain_m2 +
      INS_P.Gain1_Gain_e1 * INS_DWork.DiscreteTimeIntegrator_DSTATE_c[2]) *
      INS_P.DiscreteTimeIntegrator_gainv_mk;
  
    /* Update for DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_j[2] +=
      INS_P.DiscreteTimeIntegrator1_gainv_p * rtb_DiscreteTimeIntegrator_c__1;
  
    /* End of Outputs for SubSystem: '<S3>/IMU_Process' */
  
    /* Outputs for Atomic SubSystem: '<S3>/Mag_Process' */
    /* SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In1' incorporates:
     *  Inport: '<Root>/MAG'
     */
    rtb_uT[0] = INS_U.MAG.mag_x;
  
    /* SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In2' incorporates:
     *  Inport: '<Root>/MAG'
     */
    rtb_uT[1] = INS_U.MAG.mag_y;
  
    /* SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In3' incorporates:
     *  Inport: '<Root>/MAG'
     */
    rtb_uT[2] = INS_U.MAG.mag_z;
  
    /* Sum: '<S333>/Subtract' incorporates:
     *  Constant: '<S333>/Mag_Bias'
     *  Inport: '<Root>/MAG'
     *  SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In1'
     *  SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In2'
     *  SignalConversion: '<S273>/ConcatBufferAtVector Concatenate2In3'
     */
    rtb_DiscreteTimeIntegrator_g = INS_U.MAG.mag_x - INS_PARAM.MagBias[0];
    rtb_Relay2 = INS_U.MAG.mag_y - INS_PARAM.MagBias[1];
    rtb_Add_m = INS_U.MAG.mag_z - INS_PARAM.MagBias[2];
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      /* Gain: '<S330>/Gauss_to_uT' incorporates:
       *  Constant: '<S333>/Mag_RotMatrix'
       *  Product: '<S333>/Product1'
       */
      rtb_Product1_n[rtb_Saturation1] = INS_P.Gauss_to_uT_Gain *
        (INS_PARAM.MagRotMat[rtb_Saturation1 + 6] * rtb_Add_m +
         (INS_PARAM.MagRotMat[rtb_Saturation1 + 3] * rtb_Relay2 +
          INS_PARAM.MagRotMat[rtb_Saturation1] * rtb_DiscreteTimeIntegrator_g));
    }
  
    /* DiscreteIntegrator: '<S331>/Discrete-Time Integrator' incorporates:
     *  Inport: '<Root>/MAG'
     *  RelationalOperator: '<S335>/FixPt Relational Operator'
     *  UnitDelay: '<S335>/Delay Input1'
     */
    if (INS_U.MAG.timestamp != INS_DWork.DelayInput1_DSTATE_n) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_l =
        INS_P.DiscreteTimeIntegrator_IC_k;
    }
  
    /* RelationalOperator: '<S337>/Compare' incorporates:
     *  Constant: '<S337>/Constant'
     */
    rtb_LogicalOperator3 = (rtb_Compare_cio == INS_P.CompareToConstant_const_h);
  
    /* DataTypeConversion: '<S340>/Data Type Conversion1' incorporates:
     *  Gain: '<S340>/Gain'
     */
    rtb_Gain6 = fmod(trunc(57.295779513082323 / INS_P.wmm_lookup_res *
      INS_B.Scalefactor1), 4.294967296E+9);
  
    /* Sum: '<S340>/Sum' incorporates:
     *  Constant: '<S340>/offset_lat'
     *  DataTypeConversion: '<S340>/Data Type Conversion1'
     */
    rtb_Saturation1 = (rtb_Gain6 < 0.0 ? -(int32_T)(uint32_T)-rtb_Gain6 : (int32_T)
                       (uint32_T)rtb_Gain6) + INS_P.offset_lat_Value;
  
    /* Saturate: '<S340>/Saturation1' */
    if (rtb_Saturation1 > INS_P.Saturation1_UpperSat) {
      rtb_Saturation1 = INS_P.Saturation1_UpperSat;
    } else {
      if (rtb_Saturation1 < INS_P.Saturation1_LowerSat) {
        rtb_Saturation1 = INS_P.Saturation1_LowerSat;
      }
    }
  
    /* End of Saturate: '<S340>/Saturation1' */
  
    /* DataTypeConversion: '<S340>/Data Type Conversion2' incorporates:
     *  Gain: '<S340>/Gain6'
     */
    rtb_Gain6 = fmod(trunc(57.295779513082323 / INS_P.wmm_lookup_res *
      INS_B.Scalefactor), 4.294967296E+9);
  
    /* Sum: '<S340>/Sum1' incorporates:
     *  Constant: '<S340>/offset_lon'
     *  DataTypeConversion: '<S340>/Data Type Conversion2'
     */
    rtb_Saturation2 = (rtb_Gain6 < 0.0 ? -(int32_T)(uint32_T)-rtb_Gain6 : (int32_T)
                       (uint32_T)rtb_Gain6) + INS_P.offset_lon_Value;
  
    /* Saturate: '<S340>/Saturation2' */
    if (rtb_Saturation2 > INS_P.Saturation2_UpperSat) {
      rtb_Saturation2 = INS_P.Saturation2_UpperSat;
    } else {
      if (rtb_Saturation2 < INS_P.Saturation2_LowerSat) {
        rtb_Saturation2 = INS_P.Saturation2_LowerSat;
      }
    }
  
    /* End of Saturate: '<S340>/Saturation2' */
  
    /* LookupNDDirect: '<S340>/Declination Lookup'
     *
     * About '<S340>/Declination Lookup':
     *  2-dimensional Direct Look-Up returning a Scalar,
     */
    if (rtb_Saturation2 > 36) {
      i = 36;
    } else if (rtb_Saturation2 < 0) {
      i = 0;
    } else {
      i = rtb_Saturation2;
    }
  
    if (rtb_Saturation1 > 16) {
      rtb_Saturation1_0 = 16;
    } else if (rtb_Saturation1 < 0) {
      rtb_Saturation1_0 = 0;
    } else {
      rtb_Saturation1_0 = rtb_Saturation1;
    }
  
    /* Gain: '<S339>/Gain' incorporates:
     *  LookupNDDirect: '<S340>/Declination Lookup'
     *
     * About '<S340>/Declination Lookup':
     *  2-dimensional Direct Look-Up returning a Scalar,
     */
    rtb_DiscreteTimeIntegrator_g = (real32_T)INS_P.wmm_lookup_wmm_declination[i *
      17 + rtb_Saturation1_0] * ((real32_T)INS_P.Gain_Gain_fs * 9.53674316E-7F);
  
    /* Trigonometry: '<S348>/Trigonometric Function1' incorporates:
     *  Trigonometry: '<S348>/Trigonometric Function3'
     */
    rtb_DiscreteTimeIntegrator_df = arm_cos_f32(rtb_DiscreteTimeIntegrator_g);
    rtb_VectorConcatenate_m[0] = rtb_DiscreteTimeIntegrator_df;
  
    /* Trigonometry: '<S348>/Trigonometric Function' incorporates:
     *  Trigonometry: '<S348>/Trigonometric Function2'
     */
    rtb_Delay_m_idx_2 = arm_sin_f32(rtb_DiscreteTimeIntegrator_g);
    rtb_VectorConcatenate_m[1] = rtb_Delay_m_idx_2;
  
    /* Constant: '<S348>/Constant3' */
    rtb_VectorConcatenate_m[2] = INS_P.Constant3_Value;
  
    /* Gain: '<S348>/Gain' */
    rtb_VectorConcatenate_m[3] = INS_P.Gain_Gain_b * rtb_Delay_m_idx_2;
  
    /* Trigonometry: '<S348>/Trigonometric Function3' */
    rtb_VectorConcatenate_m[4] = rtb_DiscreteTimeIntegrator_df;
  
    /* Constant: '<S348>/Constant4' */
    rtb_VectorConcatenate_m[5] = INS_P.Constant4_Value;
  
    /* Constant: '<S348>/Constant2' */
    rtb_VectorConcatenate_m[6] = INS_P.Constant2_Value_d;
  
    /* Constant: '<S348>/Constant1' */
    rtb_VectorConcatenate_m[7] = INS_P.Constant1_Value_c2;
  
    /* Constant: '<S348>/Constant' */
    rtb_VectorConcatenate_m[8] = INS_P.Constant_Value_c;
  
    /* LookupNDDirect: '<S340>/Magnitude Lookup'
     *
     * About '<S340>/Magnitude Lookup':
     *  2-dimensional Direct Look-Up returning a Scalar,
     */
    if (rtb_Saturation2 > 36) {
      i = 36;
    } else if (rtb_Saturation2 < 0) {
      i = 0;
    } else {
      i = rtb_Saturation2;
    }
  
    if (rtb_Saturation1 > 16) {
      rtb_Saturation1_0 = 16;
    } else if (rtb_Saturation1 < 0) {
      rtb_Saturation1_0 = 0;
    } else {
      rtb_Saturation1_0 = rtb_Saturation1;
    }
  
    /* Product: '<S339>/Divide1' incorporates:
     *  Constant: '<S339>/Constant1'
     *  LookupNDDirect: '<S340>/Magnitude Lookup'
     *  MinMax: '<S339>/MinMax'
     *
     * About '<S340>/Magnitude Lookup':
     *  2-dimensional Direct Look-Up returning a Scalar,
     */
    rtb_Delay_m_idx_2 = 1.0F / fmaxf(INS_P.Constant1_Value_b,
      INS_P.wmm_lookup_wmm_magnitude[i * 17 + rtb_Saturation1_0]);
    for (i = 0; i < 3; i++) {
      /* Logic: '<S336>/AND' incorporates:
       *  Constant: '<S336>/Lower Limit'
       *  Constant: '<S336>/Upper Limit'
       *  RelationalOperator: '<S336>/Lower Test'
       *  RelationalOperator: '<S336>/Upper Test'
       */
      rtb_AND_e[i] = ((INS_P.Mag_Valid_Check_mag_min < rtb_uT[i]) && (rtb_uT[i] <
        INS_P.Mag_Valid_Check_mag_max));
  
      /* Product: '<S339>/Product1' incorporates:
       *  Delay: '<Root>/Delay'
       *  Product: '<S339>/Divide1'
       */
      rtb_Multiply_j_0[i] = INS_DWork.Delay_8_DSTATE[i + 6] * (rtb_Delay_m_idx_2 *
        rtb_Product1_n[2]) + (INS_DWork.Delay_8_DSTATE[i + 3] * (rtb_Delay_m_idx_2
        * rtb_Product1_n[1]) + rtb_Delay_m_idx_2 * rtb_Product1_n[0] *
        INS_DWork.Delay_8_DSTATE[i]);
    }
  
    for (i = 0; i < 3; i++) {
      /* Math: '<S349>/Math Function' */
      rtb_DiscreteTimeIntegrator_h[i] = rtb_Product1_n[i] * rtb_Product1_n[i];
  
      /* Product: '<S339>/Product3' */
      rtb_uT[i] = rtb_VectorConcatenate_m[i + 6] * rtb_Multiply_j_0[2] +
        (rtb_VectorConcatenate_m[i + 3] * rtb_Multiply_j_0[1] +
         rtb_VectorConcatenate_m[i] * rtb_Multiply_j_0[0]);
    }
  
    /* Sum: '<S349>/Sum of Elements' */
    i = 1;
    rtb_DiscreteTimeIntegrator_g = (rtb_DiscreteTimeIntegrator_h[0] +
      rtb_DiscreteTimeIntegrator_h[1]) + rtb_DiscreteTimeIntegrator_h[2];
  
    /* Math: '<S349>/Math Function1'
     *
     * About '<S349>/Math Function1':
     *  Operator: sqrt
     */
    if (rtb_DiscreteTimeIntegrator_g < 0.0F) {
      rtb_DiscreteTimeIntegrator_g = -sqrtf(fabsf(rtb_DiscreteTimeIntegrator_g));
    } else {
      rtb_DiscreteTimeIntegrator_g = sqrtf(rtb_DiscreteTimeIntegrator_g);
    }
  
    /* End of Math: '<S349>/Math Function1' */
  
    /* Switch: '<S349>/Switch' incorporates:
     *  Constant: '<S349>/Constant'
     *  Product: '<S349>/Product'
     */
    if (rtb_DiscreteTimeIntegrator_g > INS_P.Switch_Threshold_f) {
      rtb_Inclination = rtb_Product1_n[0];
      rtb_Delay_m_idx_1 = rtb_Product1_n[1];
      rtb_Delay_m_idx_2 = rtb_Product1_n[2];
      rtb_DiscreteTimeIntegrator_df = rtb_DiscreteTimeIntegrator_g;
    } else {
      rtb_Inclination = 0.0F;
      rtb_Delay_m_idx_1 = 0.0F;
      rtb_Delay_m_idx_2 = 0.0F;
      rtb_DiscreteTimeIntegrator_df = INS_P.Constant_Value_g2;
    }
  
    /* End of Switch: '<S349>/Switch' */
  
    /* Switch: '<S339>/Switch1' incorporates:
     *  Constant: '<S338>/default_mag_quality'
     *  Delay: '<Root>/Delay'
     *  Math: '<S339>/Math Function1'
     *  MinMax: '<S341>/MinMax'
     *  Product: '<S339>/Product2'
     *  Product: '<S349>/Divide1'
     *  Switch: '<S338>/Switch'
     *  Switch: '<S339>/Switch'
     */
    if (rtb_LogicalOperator3) {
      /* Math: '<S347>/Square' incorporates:
       *  Math: '<S345>/Square'
       */
      rtb_DiscreteTimeIntegrator_g = rtb_uT[0] * rtb_uT[0];
      rtb_Relay2 = rtb_uT[1] * rtb_uT[1];
  
      /* LookupNDDirect: '<S340>/Inclination Lookup'
       *
       * About '<S340>/Inclination Lookup':
       *  2-dimensional Direct Look-Up returning a Scalar,
       */
      if (rtb_Saturation2 > 36) {
        rtb_Saturation2 = 36;
      } else {
        if (rtb_Saturation2 < 0) {
          rtb_Saturation2 = 0;
        }
      }
  
      if (rtb_Saturation1 > 16) {
        rtb_Saturation1 = 16;
      } else {
        if (rtb_Saturation1 < 0) {
          rtb_Saturation1 = 0;
        }
      }
  
      /* Sum: '<S342>/Sum3' incorporates:
       *  Abs: '<S342>/Abs1'
       *  Constant: '<S342>/Constant2'
       *  Gain: '<S342>/Gain1'
       *  Gain: '<S342>/gain'
       *  LookupNDDirect: '<S340>/Inclination Lookup'
       *  Sqrt: '<S344>/Sqrt'
       *  Sum: '<S342>/Subtract'
       *  Sum: '<S345>/Sum of Elements'
       *  Trigonometry: '<S342>/Trigonometric Function'
       *
       * About '<S340>/Inclination Lookup':
       *  2-dimensional Direct Look-Up returning a Scalar,
       */
      u0 = INS_P.Constant2_Value_hh - fabsf((real32_T)
        (INS_P.wmm_lookup_wmm_inclination[rtb_Saturation2 * 17 + rtb_Saturation1] *
         INS_P.Gain1_Gain_o) * 0.000244140625F - atan2f(rtb_uT[2], sqrtf
        (rtb_DiscreteTimeIntegrator_g + rtb_Relay2))) * INS_P.gain_Gain_g;
  
      /* Sum: '<S343>/Sum' incorporates:
       *  Abs: '<S343>/Abs'
       *  Constant: '<S343>/Constant1'
       *  Constant: '<S343>/Nominal_Magnitude'
       *  Gain: '<S343>/gain'
       *  Math: '<S347>/Square'
       *  Sqrt: '<S346>/Sqrt'
       *  Sum: '<S343>/Subtract'
       *  Sum: '<S347>/Sum of Elements'
       */
      rtb_DiscreteTimeIntegrator_g = INS_P.Constant1_Value_cq - fabsf
        (INS_P.Nominal_Magnitude_Value - sqrtf((rtb_DiscreteTimeIntegrator_g +
           rtb_Relay2) + rtb_uT[2] * rtb_uT[2])) * INS_P.gain_Gain_m;
  
      /* Saturate: '<S342>/Saturation' */
      if (u0 > INS_P.Saturation_UpperSat_gg) {
        u0 = INS_P.Saturation_UpperSat_gg;
      } else {
        if (u0 < INS_P.Saturation_LowerSat_a) {
          u0 = INS_P.Saturation_LowerSat_a;
        }
      }
  
      /* End of Saturate: '<S342>/Saturation' */
  
      /* Saturate: '<S343>/Saturation' */
      if (rtb_DiscreteTimeIntegrator_g > INS_P.Saturation_UpperSat_gv) {
        rtb_DiscreteTimeIntegrator_g = INS_P.Saturation_UpperSat_gv;
      } else {
        if (rtb_DiscreteTimeIntegrator_g < INS_P.Saturation_LowerSat_pf) {
          rtb_DiscreteTimeIntegrator_g = INS_P.Saturation_LowerSat_pf;
        }
      }
  
      /* End of Saturate: '<S343>/Saturation' */
      rtb_DiscreteTimeIntegrator_g = fminf(u0, rtb_DiscreteTimeIntegrator_g);
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
        rtb_Product1_n[rtb_Saturation1] = INS_DWork.Delay_8_DSTATE[3 *
          rtb_Saturation1 + 2] * rtb_uT[2] + (INS_DWork.Delay_8_DSTATE[3 *
          rtb_Saturation1 + 1] * rtb_uT[1] + INS_DWork.Delay_8_DSTATE[3 *
          rtb_Saturation1] * rtb_uT[0]);
      }
    } else {
      rtb_DiscreteTimeIntegrator_g = INS_P.default_mag_quality_Value;
      rtb_Product1_n[0] = rtb_Inclination / rtb_DiscreteTimeIntegrator_df;
      rtb_Product1_n[1] = rtb_Delay_m_idx_1 / rtb_DiscreteTimeIntegrator_df;
      rtb_Product1_n[2] = rtb_Delay_m_idx_2 / rtb_DiscreteTimeIntegrator_df;
    }
  
    /* End of Switch: '<S339>/Switch1' */
  
    /* Product: '<S338>/Multiply' incorporates:
     *  Constant: '<S334>/Constant'
     *  DiscreteIntegrator: '<S331>/Discrete-Time Integrator'
     *  Logic: '<S331>/Logical Operator1'
     *  Logic: '<S331>/Logical Operator4'
     *  RelationalOperator: '<S334>/Compare'
     */
    rtb_DiscreteTimeIntegrator_g = rtb_AND_e[0] && rtb_AND_e[1] && rtb_AND_e[2] &&
      (INS_DWork.DiscreteTimeIntegrator_DSTATE_l < INS_P.Mag_Valid_Check_timeout) ?
      rtb_DiscreteTimeIntegrator_g : 0.0F;
  
    /* Update for UnitDelay: '<S335>/Delay Input1' incorporates:
     *  Inport: '<Root>/MAG'
     */
    INS_DWork.DelayInput1_DSTATE_n = INS_U.MAG.timestamp;
  
    /* Update for DiscreteIntegrator: '<S331>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S331>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTATE_l +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_l = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_l = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S331>/Discrete-Time Integrator' */
    /* End of Outputs for SubSystem: '<S3>/Mag_Process' */
  
    /* Outputs for Atomic SubSystem: '<S3>/Optical_Flow_Preprocessing' */
    /* Product: '<S274>/Multiply' incorporates:
     *  Constant: '<S274>/Constant'
     *  Delay: '<Root>/Delay'
     *  Inport: '<Root>/Optical_Flow'
     *  SignalConversion: '<S274>/TmpSignal ConversionAtMultiplyInport2'
     */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      rtb_uT[rtb_Saturation1] = INS_DWork.Delay_6_DSTATE[rtb_Saturation1 + 6] *
        INS_P.Constant_Value_bg + (INS_DWork.Delay_6_DSTATE[rtb_Saturation1 + 3] *
        INS_U.Optical_Flow.vel_y_mPs + INS_DWork.Delay_6_DSTATE[rtb_Saturation1] *
        INS_U.Optical_Flow.vel_x_mPs);
    }
  
    /* End of Product: '<S274>/Multiply' */
  
    /* DiscreteIntegrator: '<S350>/Discrete-Time Integrator' incorporates:
     *  Inport: '<Root>/Optical_Flow'
     *  RelationalOperator: '<S352>/FixPt Relational Operator'
     *  UnitDelay: '<S352>/Delay Input1'
     */
    if (INS_U.Optical_Flow.timestamp_ms != INS_DWork.DelayInput1_DSTATE_l) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_n =
        INS_P.DiscreteTimeIntegrator_IC_g;
    }
  
    /* Logic: '<S350>/Logical Operator' incorporates:
     *  Constant: '<S351>/Constant'
     *  DiscreteIntegrator: '<S350>/Discrete-Time Integrator'
     *  Inport: '<Root>/Optical_Flow'
     *  RelationalOperator: '<S351>/Compare'
     */
    rtb_LogicalOperator3 = ((INS_DWork.DiscreteTimeIntegrator_DSTATE_n <
      INS_P.CompareToConstant4_const_k) && (INS_U.Optical_Flow.valid != 0U));
  
    /* Update for UnitDelay: '<S352>/Delay Input1' incorporates:
     *  Inport: '<Root>/Optical_Flow'
     */
    INS_DWork.DelayInput1_DSTATE_l = INS_U.Optical_Flow.timestamp_ms;
  
    /* Update for DiscreteIntegrator: '<S350>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S350>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTATE_n +
      INS_P.Constant_Value_jp;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      if (rtb_Delay_m_idx_2 >= 0.0F) {
        INS_DWork.DiscreteTimeIntegrator_DSTATE_n = (uint32_T)rtb_Delay_m_idx_2;
      } else {
        INS_DWork.DiscreteTimeIntegrator_DSTATE_n = 0U;
      }
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_n = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S350>/Discrete-Time Integrator' */
    /* End of Outputs for SubSystem: '<S3>/Optical_Flow_Preprocessing' */
  
    /* Outputs for Atomic SubSystem: '<S3>/Sonar_Process' */
    /* RelationalOperator: '<S356>/FixPt Relational Operator' incorporates:
     *  Inport: '<Root>/Sonar'
     *  UnitDelay: '<S356>/Delay Input1'
     */
    rtb_heading_quality = (INS_U.Sonar.timestamp_ms !=
      INS_DWork.DelayInput1_DSTATE);
  
    /* DiscreteIntegrator: '<S353>/Discrete-Time Integrator' */
    if (rtb_heading_quality) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kn =
        INS_P.DiscreteTimeIntegrator_IC_pt;
    }
  
    /* Logic: '<S353>/Logical Operator' incorporates:
     *  Constant: '<S354>/Constant'
     *  Constant: '<S355>/Constant'
     *  Constant: '<S357>/Lower Limit'
     *  Constant: '<S357>/Upper Limit'
     *  Delay: '<Root>/Delay1'
     *  Delay: '<S353>/Delay'
     *  DiscreteIntegrator: '<S353>/Discrete-Time Integrator'
     *  Inport: '<Root>/Sonar'
     *  Logic: '<S357>/AND'
     *  Math: '<S353>/Square'
     *  Math: '<S353>/Square1'
     *  RelationalOperator: '<S354>/Compare'
     *  RelationalOperator: '<S355>/Compare'
     *  RelationalOperator: '<S357>/Lower Test'
     *  RelationalOperator: '<S357>/Upper Test'
     *  Sqrt: '<S353>/Sqrt'
     *  Sum: '<S353>/Add'
     */
    rtb_LogicalOperator_i = ((INS_DWork.DiscreteTimeIntegrator_DSTAT_kn <
      INS_P.CompareToConstant4_const_h) && (INS_DWork.Delay_DSTATE_dz != 0) &&
      ((INS_P.IntervalTest_lowlimit_d < INS_U.Sonar.distance_m) &&
       (INS_U.Sonar.distance_m <= INS_P.IntervalTest_uplimit_a)) && (sqrtf
      (INS_DWork.Delay1_4_DSTATE * INS_DWork.Delay1_4_DSTATE +
       INS_DWork.Delay1_5_DSTATE * INS_DWork.Delay1_5_DSTATE) <
      INS_P.CompareToConstant_const_f));
  
    /* Update for UnitDelay: '<S356>/Delay Input1' incorporates:
     *  Inport: '<Root>/Sonar'
     */
    INS_DWork.DelayInput1_DSTATE = INS_U.Sonar.timestamp_ms;
  
    /* Update for DiscreteIntegrator: '<S353>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S353>/Constant'
     */
    rtb_Delay_m_idx_2 = (real32_T)INS_DWork.DiscreteTimeIntegrator_DSTAT_kn +
      (real32_T)INS_EXPORT.period;
    if (rtb_Delay_m_idx_2 < 4.2949673E+9F) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kn = (uint32_T)rtb_Delay_m_idx_2;
    } else {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kn = MAX_uint32_T;
    }
  
    /* End of Update for DiscreteIntegrator: '<S353>/Discrete-Time Integrator' */
  
    /* Switch: '<S353>/Switch' */
    if (rtb_heading_quality) {
      /* Update for Delay: '<S353>/Delay' incorporates:
       *  Constant: '<S353>/Constant1'
       */
      INS_DWork.Delay_DSTATE_dz = INS_P.Constant1_Value_l;
    }
  
    /* End of Switch: '<S353>/Switch' */
    /* End of Outputs for SubSystem: '<S3>/Sonar_Process' */
    /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S56>/Attitude_Init' */
    /* Sqrt: '<S110>/Sqrt' incorporates:
     *  Math: '<S115>/Square'
     *  Sum: '<S115>/Sum of Elements'
     */
    rtb_DiscreteTimeIntegrator_df = sqrtf((rtb_Switch3[0] * rtb_Switch3[0] +
      rtb_Switch3[1] * rtb_Switch3[1]) + rtb_Switch3[2] * rtb_Switch3[2]);
  
    /* Sum: '<S109>/Add' incorporates:
     *  Constant: '<S107>/Constant'
     *  Constant: '<S108>/Lower Limit'
     *  Constant: '<S108>/Upper Limit'
     *  Constant: '<S62>/Constant'
     *  Delay: '<S109>/Delay'
     *  Logic: '<S108>/AND'
     *  Logic: '<S64>/Logical Operator1'
     *  Math: '<S116>/Square'
     *  RelationalOperator: '<S107>/Compare'
     *  RelationalOperator: '<S108>/Lower Test'
     *  RelationalOperator: '<S108>/Upper Test'
     *  RelationalOperator: '<S62>/Compare'
     *  Sqrt: '<S111>/Sqrt'
     *  Sum: '<S116>/Sum of Elements'
     */
    rtb_Saturation_p3 = (uint16_T)((uint32_T)(rtb_Compare_fz &&
      (rtb_DiscreteTimeIntegrator_g >= INS_P.CompareToConstant_const_g) &&
      ((INS_P.IntervalTest_lowlimit <= rtb_DiscreteTimeIntegrator_df) &&
       (rtb_DiscreteTimeIntegrator_df <= INS_P.IntervalTest_uplimit)) && (sqrtf
      ((rtb_Product1_n[0] * rtb_Product1_n[0] + rtb_Product1_n[1] *
        rtb_Product1_n[1]) + rtb_Product1_n[2] * rtb_Product1_n[2]) >
      INS_P.CompareToConstant_const_p)) + INS_DWork.Delay_DSTATE_i);
  
    /* Saturate: '<S109>/Saturation' */
    if (rtb_Saturation_p3 > INS_P.Saturation_UpperSat_k5) {
      rtb_Saturation_p3 = INS_P.Saturation_UpperSat_k5;
    } else {
      if (rtb_Saturation_p3 < INS_P.Saturation_LowerSat_hb) {
        rtb_Saturation_p3 = INS_P.Saturation_LowerSat_hb;
      }
    }
  
    /* End of Saturate: '<S109>/Saturation' */
  
    /* RelationalOperator: '<S114>/Compare' incorporates:
     *  Constant: '<S113>/Constant'
     *  Constant: '<S114>/Constant'
     *  RelationalOperator: '<S113>/Compare'
     */
    rtb_Delay_b = ((rtb_Saturation_p3 > INS_P.valid_samples_const) > (int32_T)
                   INS_P.Constant_Value_af);
  
    /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
    /* Logic: '<S64>/Logical Operator' incorporates:
     *  Inport: '<Root>/reset'
     *  RelationalOperator: '<S112>/FixPt Relational Operator'
     *  UnitDelay: '<S112>/Delay Input1'
     */
    rtb_heading_quality = (((int32_T)rtb_Delay_b > (int32_T)
      INS_DWork.DelayInput1_DSTATE_p) || (INS_U.reset != 0));
  
    /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
    /* Outputs for Enabled SubSystem: '<S58>/Attitude_Init' incorporates:
     *  EnablePort: '<S61>/Enable'
     */
    if (rtb_heading_quality) {
      /* Gain: '<S61>/Gain' */
      rtb_ba_O_mDs2[0] = INS_P.Gain_Gain_d * rtb_Switch3[0];
      rtb_ba_O_mDs2[1] = INS_P.Gain_Gain_d * rtb_Switch3[1];
      rtb_ba_O_mDs2[2] = INS_P.Gain_Gain_d * rtb_Switch3[2];
  
      /* Sum: '<S66>/Sum' incorporates:
       *  Product: '<S96>/Multiply'
       *  Product: '<S96>/Multiply1'
       *  Product: '<S96>/Multiply2'
       *  Product: '<S97>/Multiply'
       *  Product: '<S97>/Multiply1'
       *  Product: '<S97>/Multiply2'
       */
      rtb_Multiply_j[0] = rtb_ba_O_mDs2[1] * rtb_Product1_n[2] - rtb_ba_O_mDs2[2] *
        rtb_Product1_n[1];
      rtb_Multiply_j[1] = rtb_ba_O_mDs2[2] * rtb_Product1_n[0] - rtb_ba_O_mDs2[0] *
        rtb_Product1_n[2];
      rtb_Relay2 = rtb_ba_O_mDs2[0] * rtb_Product1_n[1] - rtb_ba_O_mDs2[1] *
        rtb_Product1_n[0];
  
      /* Sum: '<S67>/Sum' incorporates:
       *  Product: '<S98>/Multiply'
       *  Product: '<S99>/Multiply'
       */
      u0 = rtb_Multiply_j[1] * rtb_ba_O_mDs2[2] - rtb_Relay2 * rtb_ba_O_mDs2[1];
      rtb_Divide1_m[0] = u0;
  
      /* Math: '<S100>/Math Function' */
      rtb_MatrixConcatenate[0] = u0 * u0;
  
      /* Sum: '<S67>/Sum' incorporates:
       *  Product: '<S98>/Multiply1'
       *  Product: '<S99>/Multiply1'
       */
      u0 = rtb_Relay2 * rtb_ba_O_mDs2[0] - rtb_Multiply_j[0] * rtb_ba_O_mDs2[2];
      rtb_Divide1_m[1] = u0;
  
      /* Math: '<S100>/Math Function' */
      rtb_MatrixConcatenate[1] = u0 * u0;
  
      /* Sum: '<S67>/Sum' incorporates:
       *  Product: '<S98>/Multiply2'
       *  Product: '<S99>/Multiply2'
       */
      u0 = rtb_Multiply_j[0] * rtb_ba_O_mDs2[1] - rtb_Multiply_j[1] *
        rtb_ba_O_mDs2[0];
  
      /* MinMax: '<S68>/Max' incorporates:
       *  Constant: '<S68>/Constant2'
       *  Math: '<S100>/Math Function'
       *  Sqrt: '<S100>/Sqrt'
       *  Sum: '<S100>/Sum of Elements'
       */
      rtb_Inclination = fmaxf(sqrtf((rtb_MatrixConcatenate[0] +
        rtb_MatrixConcatenate[1]) + u0 * u0), INS_P.Constant2_Value_h);
  
      /* Product: '<S68>/Divide' */
      rtb_VectorConcatenate_m[0] = rtb_Divide1_m[0] / rtb_Inclination;
      rtb_VectorConcatenate_m[1] = rtb_Divide1_m[1] / rtb_Inclination;
      rtb_VectorConcatenate_m[2] = u0 / rtb_Inclination;
  
      /* MinMax: '<S69>/Max' incorporates:
       *  Constant: '<S69>/Constant2'
       *  Math: '<S101>/Math Function'
       *  Sqrt: '<S101>/Sqrt'
       *  Sum: '<S101>/Sum of Elements'
       */
      rtb_Inclination = fmaxf(sqrtf((rtb_Multiply_j[0] * rtb_Multiply_j[0] +
        rtb_Multiply_j[1] * rtb_Multiply_j[1]) + rtb_Relay2 * rtb_Relay2),
        INS_P.Constant2_Value_m);
  
      /* Product: '<S69>/Divide' */
      rtb_VectorConcatenate_m[3] = rtb_Multiply_j[0] / rtb_Inclination;
      rtb_VectorConcatenate_m[4] = rtb_Multiply_j[1] / rtb_Inclination;
      rtb_VectorConcatenate_m[5] = rtb_Relay2 / rtb_Inclination;
  
      /* MinMax: '<S70>/Max' incorporates:
       *  Constant: '<S70>/Constant2'
       *  Math: '<S102>/Math Function'
       *  Sqrt: '<S102>/Sqrt'
       *  Sum: '<S102>/Sum of Elements'
       */
      rtb_DiscreteTimeIntegrator_df = fmaxf(sqrtf((rtb_ba_O_mDs2[0] *
        rtb_ba_O_mDs2[0] + rtb_ba_O_mDs2[1] * rtb_ba_O_mDs2[1]) + rtb_ba_O_mDs2[2]
        * rtb_ba_O_mDs2[2]), INS_P.Constant2_Value_gc);
  
      /* Product: '<S70>/Divide' */
      rtb_VectorConcatenate_m[6] = rtb_ba_O_mDs2[0] /
        rtb_DiscreteTimeIntegrator_df;
      rtb_VectorConcatenate_m[7] = rtb_ba_O_mDs2[1] /
        rtb_DiscreteTimeIntegrator_df;
      rtb_VectorConcatenate_m[8] = rtb_ba_O_mDs2[2] /
        rtb_DiscreteTimeIntegrator_df;
  
      /* Sum: '<S73>/Add' */
      rtb_Add_m = (rtb_VectorConcatenate_m[0] + rtb_VectorConcatenate_m[4]) +
        rtb_VectorConcatenate_m[8];
  
      /* If: '<S65>/If' incorporates:
       *  If: '<S71>/Find Maximum Diagonal Value'
       */
      if (rtb_Add_m > 0.0F) {
        /* Outputs for IfAction SubSystem: '<S65>/PositiveTrace' incorporates:
         *  ActionPort: '<S72>/Action Port'
         */
        /* Sqrt: '<S72>/sqrt' incorporates:
         *  Constant: '<S72>/Constant'
         *  Sum: '<S72>/Sum'
         */
        rtb_DiscreteTimeIntegrator_df = sqrtf(rtb_Add_m + INS_P.Constant_Value_e);
  
        /* Gain: '<S72>/Gain' */
        INS_B.Merge[0] = INS_P.Gain_Gain_o * rtb_DiscreteTimeIntegrator_df;
  
        /* Gain: '<S72>/Gain1' */
        rtb_DiscreteTimeIntegrator_df *= INS_P.Gain1_Gain_h;
  
        /* Product: '<S72>/Product' incorporates:
         *  Sum: '<S93>/Add'
         *  Sum: '<S94>/Add'
         *  Sum: '<S95>/Add'
         */
        INS_B.Merge[1] = (rtb_VectorConcatenate_m[7] - rtb_VectorConcatenate_m[5])
          / rtb_DiscreteTimeIntegrator_df;
        INS_B.Merge[2] = (rtb_VectorConcatenate_m[2] - rtb_VectorConcatenate_m[6])
          / rtb_DiscreteTimeIntegrator_df;
        INS_B.Merge[3] = (rtb_VectorConcatenate_m[3] - rtb_VectorConcatenate_m[1])
          / rtb_DiscreteTimeIntegrator_df;
  
        /* End of Outputs for SubSystem: '<S65>/PositiveTrace' */
      } else {
        /* Outputs for IfAction SubSystem: '<S65>/NegativeTrace' incorporates:
         *  ActionPort: '<S71>/Action Port'
         */
        if ((rtb_VectorConcatenate_m[4] > rtb_VectorConcatenate_m[0]) &&
            (rtb_VectorConcatenate_m[4] > rtb_VectorConcatenate_m[8])) {
          /* Outputs for IfAction SubSystem: '<S71>/Maximum Value at DCM(2,2)' incorporates:
           *  ActionPort: '<S75>/Action Port'
           */
          /* If: '<S71>/Find Maximum Diagonal Value' incorporates:
           *  Constant: '<S86>/Constant1'
           *  Constant: '<S86>/Constant2'
           *  Constant: '<S87>/Constant'
           *  Gain: '<S75>/Gain'
           *  Gain: '<S75>/Gain1'
           *  Gain: '<S75>/Gain3'
           *  Gain: '<S75>/Gain4'
           *  Product: '<S75>/Product'
           *  Product: '<S86>/Product'
           *  Sqrt: '<S75>/sqrt'
           *  Sum: '<S83>/Add'
           *  Sum: '<S84>/Add'
           *  Sum: '<S85>/Add'
           *  Sum: '<S87>/Add'
           *  Switch: '<S86>/Switch'
           */
          rtb_DiscreteTimeIntegrator_df = sqrtf(((rtb_VectorConcatenate_m[4] -
            rtb_VectorConcatenate_m[0]) - rtb_VectorConcatenate_m[8]) +
            INS_P.Constant_Value_o);
          INS_B.Merge[2] = INS_P.Gain_Gain_e * rtb_DiscreteTimeIntegrator_df;
          if (rtb_DiscreteTimeIntegrator_df != 0.0F) {
            rtb_Square_l_idx_0 = INS_P.Constant1_Value;
            rtb_Square_l_idx_1 = rtb_DiscreteTimeIntegrator_df;
          } else {
            rtb_Square_l_idx_0 = INS_P.Constant2_Value[0];
            rtb_Square_l_idx_1 = INS_P.Constant2_Value[1];
          }
  
          rtb_DiscreteTimeIntegrator_df = rtb_Square_l_idx_0 / rtb_Square_l_idx_1;
          INS_B.Merge[1] = (rtb_VectorConcatenate_m[1] + rtb_VectorConcatenate_m[3])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain1_Gain_j;
          INS_B.Merge[3] = (rtb_VectorConcatenate_m[5] + rtb_VectorConcatenate_m[7])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain3_Gain;
          INS_B.Merge[0] = (rtb_VectorConcatenate_m[2] - rtb_VectorConcatenate_m[6])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain4_Gain;
  
          /* End of Outputs for SubSystem: '<S71>/Maximum Value at DCM(2,2)' */
        } else if (rtb_VectorConcatenate_m[8] > rtb_VectorConcatenate_m[0]) {
          /* Outputs for IfAction SubSystem: '<S71>/Maximum Value at DCM(3,3)' incorporates:
           *  ActionPort: '<S76>/Action Port'
           */
          /* If: '<S71>/Find Maximum Diagonal Value' incorporates:
           *  Constant: '<S91>/Constant1'
           *  Constant: '<S91>/Constant2'
           *  Constant: '<S92>/Constant'
           *  Gain: '<S76>/Gain'
           *  Gain: '<S76>/Gain1'
           *  Gain: '<S76>/Gain2'
           *  Gain: '<S76>/Gain3'
           *  Product: '<S76>/Product'
           *  Product: '<S91>/Product'
           *  Sqrt: '<S76>/sqrt'
           *  Sum: '<S88>/Add'
           *  Sum: '<S89>/Add'
           *  Sum: '<S90>/Add'
           *  Sum: '<S92>/Add'
           *  Switch: '<S91>/Switch'
           */
          rtb_DiscreteTimeIntegrator_df = sqrtf(((rtb_VectorConcatenate_m[8] -
            rtb_VectorConcatenate_m[0]) - rtb_VectorConcatenate_m[4]) +
            INS_P.Constant_Value_jf);
          INS_B.Merge[3] = INS_P.Gain_Gain_j * rtb_DiscreteTimeIntegrator_df;
          if (rtb_DiscreteTimeIntegrator_df != 0.0F) {
            rtb_Square_l_idx_0 = INS_P.Constant1_Value_n;
            rtb_Square_l_idx_1 = rtb_DiscreteTimeIntegrator_df;
          } else {
            rtb_Square_l_idx_0 = INS_P.Constant2_Value_i[0];
            rtb_Square_l_idx_1 = INS_P.Constant2_Value_i[1];
          }
  
          rtb_DiscreteTimeIntegrator_df = rtb_Square_l_idx_0 / rtb_Square_l_idx_1;
          INS_B.Merge[1] = (rtb_VectorConcatenate_m[2] + rtb_VectorConcatenate_m[6])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain1_Gain_l;
          INS_B.Merge[2] = (rtb_VectorConcatenate_m[5] + rtb_VectorConcatenate_m[7])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain2_Gain_d;
          INS_B.Merge[0] = (rtb_VectorConcatenate_m[3] - rtb_VectorConcatenate_m[1])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain3_Gain_l;
  
          /* End of Outputs for SubSystem: '<S71>/Maximum Value at DCM(3,3)' */
        } else {
          /* Outputs for IfAction SubSystem: '<S71>/Maximum Value at DCM(1,1)' incorporates:
           *  ActionPort: '<S74>/Action Port'
           */
          /* If: '<S71>/Find Maximum Diagonal Value' incorporates:
           *  Constant: '<S81>/Constant1'
           *  Constant: '<S81>/Constant2'
           *  Constant: '<S82>/Constant'
           *  Gain: '<S74>/Gain'
           *  Gain: '<S74>/Gain1'
           *  Gain: '<S74>/Gain2'
           *  Gain: '<S74>/Gain3'
           *  Product: '<S74>/Product'
           *  Product: '<S81>/Product'
           *  Sqrt: '<S74>/sqrt'
           *  Sum: '<S78>/Add'
           *  Sum: '<S79>/Add'
           *  Sum: '<S80>/Add'
           *  Sum: '<S82>/Add'
           *  Switch: '<S81>/Switch'
           */
          rtb_DiscreteTimeIntegrator_df = sqrtf(((rtb_VectorConcatenate_m[0] -
            rtb_VectorConcatenate_m[4]) - rtb_VectorConcatenate_m[8]) +
            INS_P.Constant_Value_g);
          INS_B.Merge[1] = INS_P.Gain_Gain_m * rtb_DiscreteTimeIntegrator_df;
          if (rtb_DiscreteTimeIntegrator_df != 0.0F) {
            rtb_Square_l_idx_0 = INS_P.Constant1_Value_c;
            rtb_Square_l_idx_1 = rtb_DiscreteTimeIntegrator_df;
          } else {
            rtb_Square_l_idx_0 = INS_P.Constant2_Value_g[0];
            rtb_Square_l_idx_1 = INS_P.Constant2_Value_g[1];
          }
  
          rtb_DiscreteTimeIntegrator_df = rtb_Square_l_idx_0 / rtb_Square_l_idx_1;
          INS_B.Merge[2] = (rtb_VectorConcatenate_m[1] + rtb_VectorConcatenate_m[3])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain1_Gain_b;
          INS_B.Merge[3] = (rtb_VectorConcatenate_m[2] + rtb_VectorConcatenate_m[6])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain2_Gain_o;
          INS_B.Merge[0] = (rtb_VectorConcatenate_m[7] - rtb_VectorConcatenate_m[5])
            * rtb_DiscreteTimeIntegrator_df * INS_P.Gain3_Gain_d;
  
          /* End of Outputs for SubSystem: '<S71>/Maximum Value at DCM(1,1)' */
        }
  
        /* End of Outputs for SubSystem: '<S65>/NegativeTrace' */
      }
  
      /* End of If: '<S65>/If' */
  
      /* SignalConversion: '<S61>/OutportBufferForquaternion_0' */
      INS_B.OutportBufferForquaternion_0[0] = INS_B.Merge[0];
      INS_B.OutportBufferForquaternion_0[1] = INS_B.Merge[1];
      INS_B.OutportBufferForquaternion_0[2] = INS_B.Merge[2];
      INS_B.OutportBufferForquaternion_0[3] = INS_B.Merge[3];
    }
  
    /* End of Outputs for SubSystem: '<S58>/Attitude_Init' */
  
    /* Sum: '<S103>/Sum4' incorporates:
     *  DiscreteIntegrator: '<S106>/Discrete-Time Integrator5'
     *  Sum: '<S106>/Sum9'
     */
    rtb_Add_m = rtb_VectorConcatenate_idx_0 -
      INS_DWork.DiscreteTimeIntegrator5_DSTATE[0];
  
    /* Abs: '<S63>/Abs' incorporates:
     *  DiscreteIntegrator: '<S106>/Discrete-Time Integrator5'
     *  Sum: '<S103>/Sum4'
     */
    rtb_Square_l_idx_1 = fabsf(rtb_VectorConcatenate_idx_1 -
      INS_DWork.DiscreteTimeIntegrator5_DSTATE[1]);
  
    /* Saturate: '<S63>/Saturation2' */
    if (INS_DWork.DiscreteTimeIntegrator5_DSTAT_o > INS_P.Saturation2_UpperSat_i)
    {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation2_UpperSat_i;
    } else if (INS_DWork.DiscreteTimeIntegrator5_DSTAT_o <
               INS_P.Saturation2_LowerSat_g) {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation2_LowerSat_g;
    } else {
      rtb_DiscreteTimeIntegrator_df = INS_DWork.DiscreteTimeIntegrator5_DSTAT_o;
    }
  
    /* End of Saturate: '<S63>/Saturation2' */
  
    /* Relay: '<S63>/Relay2' incorporates:
     *  DiscreteIntegrator: '<S105>/Discrete-Time Integrator5'
     */
    if (INS_DWork.DiscreteTimeIntegrator5_DSTAT_n >= INS_P.Relay2_OnVal) {
      INS_DWork.Relay2_Mode = true;
    } else {
      if (INS_DWork.DiscreteTimeIntegrator5_DSTAT_n <= INS_P.Relay2_OffVal) {
        INS_DWork.Relay2_Mode = false;
      }
    }
  
    if (INS_DWork.Relay2_Mode) {
      rtb_Relay2 = INS_P.Relay2_YOn;
    } else {
      rtb_Relay2 = INS_P.Relay2_YOff;
    }
  
    /* End of Relay: '<S63>/Relay2' */
  
    /* Update for Delay: '<S109>/Delay' */
    INS_DWork.Delay_DSTATE_i = rtb_Saturation_p3;
  
    /* Update for UnitDelay: '<S112>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_p = rtb_Delay_b;
  
    /* Update for DiscreteIntegrator: '<S106>/Discrete-Time Integrator5' incorporates:
     *  Gain: '<S106>/Gain2'
     *  Sum: '<S106>/Sum9'
     */
    INS_DWork.DiscreteTimeIntegrator5_DSTATE[0] += rtb_Add_m * INS_P.Gain2_Gain_h *
      INS_P.DiscreteTimeIntegrator5_gainval;
    INS_DWork.DiscreteTimeIntegrator5_DSTATE[1] += (rtb_VectorConcatenate_idx_1 -
      INS_DWork.DiscreteTimeIntegrator5_DSTATE[1]) * INS_P.Gain2_Gain_h *
      INS_P.DiscreteTimeIntegrator5_gainval;
  
    /* Update for DiscreteIntegrator: '<S104>/Discrete-Time Integrator5' incorporates:
     *  Abs: '<S63>/Abs'
     *  Gain: '<S104>/Gain2'
     *  MinMax: '<S63>/MinMax1'
     *  Sum: '<S103>/Sum4'
     *  Sum: '<S104>/Sum9'
     */
    INS_DWork.DiscreteTimeIntegrator5_DSTAT_o += (fmaxf(fabsf(rtb_Add_m),
      rtb_Square_l_idx_1) - INS_DWork.DiscreteTimeIntegrator5_DSTAT_o) *
      INS_P.Gain2_Gain_c * INS_P.DiscreteTimeIntegrator5_gainv_o;
  
    /* Update for DiscreteIntegrator: '<S105>/Discrete-Time Integrator5' incorporates:
     *  Gain: '<S105>/Gain2'
     *  Sum: '<S105>/Sum9'
     */
    INS_DWork.DiscreteTimeIntegrator5_DSTAT_n += (rtb_DiscreteTimeIntegrator_df -
      INS_DWork.DiscreteTimeIntegrator5_DSTAT_n) * INS_P.Gain2_Gain_f *
      INS_P.DiscreteTimeIntegrator5_gainv_b;
  
    /* End of Outputs for SubSystem: '<S56>/Attitude_Init' */
  
    /* Outputs for Atomic SubSystem: '<S56>/Correct' */
    /* Product: '<S122>/Multiply' incorporates:
     *  Delay: '<S56>/Delay'
     */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      rtb_MatrixConcatenate[rtb_Saturation1] =
        INS_DWork.Delay_8_DSTATE_l[rtb_Saturation1 + 6] * rtb_Switch3[2] +
        (INS_DWork.Delay_8_DSTATE_l[rtb_Saturation1 + 3] * rtb_Switch3[1] +
         INS_DWork.Delay_8_DSTATE_l[rtb_Saturation1] * rtb_Switch3[0]);
    }
  
    /* End of Product: '<S122>/Multiply' */
  
    /* DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' */
    if (INS_DWork.DiscreteTimeIntegrator1_IC_LO_o != 0) {
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[0] = rtb_MatrixConcatenate[0];
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[1] = rtb_MatrixConcatenate[1];
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[2] = rtb_MatrixConcatenate[2];
    }
  
    rtb_DiscreteTimeIntegrator_h[0] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[0];
    rtb_DiscreteTimeIntegrator_h[1] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[1];
    rtb_DiscreteTimeIntegrator_h[2] = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[2];
  
    /* Relay: '<S141>/Relay' */
    if (rtb_Sum2_j >= INS_P.Relay_OnVal) {
      INS_DWork.Relay_Mode_n = true;
    } else {
      if (rtb_Sum2_j <= INS_P.Relay_OffVal) {
        INS_DWork.Relay_Mode_n = false;
      }
    }
  
    if (INS_DWork.Relay_Mode_n) {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOn;
    } else {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOff;
    }
  
    /* End of Relay: '<S141>/Relay' */
  
    /* Logic: '<S141>/Logical Operator1' incorporates:
     *  Logic: '<S141>/Logical Operator'
     */
    rtb_Delay_b = ((!rtb_Compare_cio) && (rtb_Delay_m_idx_2 != 0.0F));
  
    /* Sum: '<S141>/Sum' */
    rtb_Compare_cl = (uint8_T)(rtb_LogicalOperator3 - rtb_Delay_b);
  
    /* Saturate: '<S141>/Saturation' */
    if (rtb_Compare_cl > INS_P.Saturation_UpperSat_d) {
      rtb_Compare_cl = INS_P.Saturation_UpperSat_d;
    } else {
      if (rtb_Compare_cl < INS_P.Saturation_LowerSat_b) {
        rtb_Compare_cl = INS_P.Saturation_LowerSat_b;
      }
    }
  
    /* End of Saturate: '<S141>/Saturation' */
  
    /* Sum: '<S143>/Sum1' incorporates:
     *  Constant: '<S143>/GPS_Delay'
     *  Constant: '<S143>/OptFlow_Delay'
     *  Product: '<S143>/Multiply2'
     *  Product: '<S143>/Multiply3'
     */
    rtb_DiscreteTimeIntegrator_df = (real32_T)(rtb_Compare_cl *
      INS_P.OptFlow_Delay_Value + (rtb_Delay_b ? (int32_T)INS_P.GPS_Delay_Value :
      0));
  
    /* Delay: '<S122>/Delay' incorporates:
     *  DiscreteIntegrator: '<S138>/Discrete-Time Integrator1'
     */
    if (rtb_DiscreteTimeIntegrator_df < 1.0F) {
      rtb_Inclination = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[0];
      rtb_Delay_m_idx_1 = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[1];
      rtb_Delay_m_idx_2 = INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[2];
    } else {
      if (rtb_DiscreteTimeIntegrator_df > 200.0F) {
        rtb_Saturation1 = 200;
      } else {
        rtb_Saturation1 = (int32_T)fmodf(rtb_DiscreteTimeIntegrator_df,
          4.2949673E+9F);
      }
  
      rtb_Saturation1 = (int32_T)(200U - rtb_Saturation1) * 3;
      rtb_Inclination = INS_DWork.Delay_DSTATE_lv[rtb_Saturation1];
      rtb_Delay_m_idx_1 = INS_DWork.Delay_DSTATE_lv[1 + rtb_Saturation1];
      rtb_Delay_m_idx_2 = INS_DWork.Delay_DSTATE_lv[2 + rtb_Saturation1];
    }
  
    /* End of Delay: '<S122>/Delay' */
  
    /* RelationalOperator: '<S139>/FixPt Relational Operator' incorporates:
     *  UnitDelay: '<S139>/Delay Input1'
     */
    rtb_FixPtRelationalOperator_l = ((int32_T)rtb_Delay_b > (int32_T)
      INS_DWork.DelayInput1_DSTATE_ff);
  
    /* Outputs for Enabled SubSystem: '<S123>/GPS_Acc_Observer' incorporates:
     *  EnablePort: '<S140>/Enable'
     */
    if (rtb_Delay_b) {
      if (!INS_DWork.GPS_Acc_Observer_MODE) {
        /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' */
        INS_DWork.DiscreteTimeIntegrator1_IC_L_oi = 1U;
        INS_DWork.DiscreteTimeIntegrator1_PrevRes = 0;
  
        /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[0] =
          INS_P.DiscreteTimeIntegrator_IC_a;
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[1] =
          INS_P.DiscreteTimeIntegrator_IC_a;
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[2] =
          INS_P.DiscreteTimeIntegrator_IC_a;
        INS_DWork.DiscreteTimeIntegrator_PrevRe_c = 0;
        INS_DWork.GPS_Acc_Observer_MODE = true;
      }
  
      /* DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' */
      if (INS_DWork.DiscreteTimeIntegrator1_IC_L_oi != 0) {
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[0] = rtb_vel_N;
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[1] = rtb_vel_E;
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[2] = rtb_vel_D;
      }
  
      if (rtb_FixPtRelationalOperator_l ||
          (INS_DWork.DiscreteTimeIntegrator1_PrevRes != 0)) {
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[0] = rtb_vel_N;
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[1] = rtb_vel_E;
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[2] = rtb_vel_D;
      }
  
      /* Sum: '<S144>/Subtract' incorporates:
       *  DiscreteIntegrator: '<S144>/Discrete-Time Integrator1'
       */
      rtb_Multiply_j[0] = rtb_vel_N - INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[0];
      rtb_Multiply_j[1] = rtb_vel_E - INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[1];
      rtb_Multiply_j[2] = rtb_vel_D - INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[2];
  
      /* DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      if (rtb_FixPtRelationalOperator_l ||
          (INS_DWork.DiscreteTimeIntegrator_PrevRe_c != 0)) {
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[0] =
          INS_P.DiscreteTimeIntegrator_IC_a;
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[1] =
          INS_P.DiscreteTimeIntegrator_IC_a;
        INS_DWork.DiscreteTimeIntegrator_DSTATE_o[2] =
          INS_P.DiscreteTimeIntegrator_IC_a;
      }
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_IC_L_oi = 0U;
      INS_DWork.DiscreteTimeIntegrator1_PrevRes = (int8_T)
        rtb_FixPtRelationalOperator_l;
  
      /* DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_B.DiscreteTimeIntegrator[0] = INS_DWork.DiscreteTimeIntegrator_DSTATE_o
        [0];
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' incorporates:
       *  Gain: '<S144>/Gain2'
       *  Sum: '<S144>/Add'
       */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[0] += (INS_P.Gain2_Gain_a *
        rtb_Multiply_j[0] + INS_B.DiscreteTimeIntegrator[0]) *
        INS_P.DiscreteTimeIntegrator1_gainval;
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' incorporates:
       *  DiscreteIntegrator: '<S144>/Discrete-Time Integrator1'
       *  Gain: '<S144>/Gain2'
       *  Gain: '<S144>/Gain3'
       */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[0] += INS_P.Gain3_Gain_p *
        rtb_Multiply_j[0] * INS_P.DiscreteTimeIntegrator_gainv_i0;
  
      /* DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_B.DiscreteTimeIntegrator[1] = INS_DWork.DiscreteTimeIntegrator_DSTATE_o
        [1];
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' incorporates:
       *  Gain: '<S144>/Gain2'
       *  Sum: '<S144>/Add'
       */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[1] += (INS_P.Gain2_Gain_a *
        rtb_Multiply_j[1] + INS_B.DiscreteTimeIntegrator[1]) *
        INS_P.DiscreteTimeIntegrator1_gainval;
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' incorporates:
       *  DiscreteIntegrator: '<S144>/Discrete-Time Integrator1'
       *  Gain: '<S144>/Gain2'
       *  Gain: '<S144>/Gain3'
       */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[1] += INS_P.Gain3_Gain_p *
        rtb_Multiply_j[1] * INS_P.DiscreteTimeIntegrator_gainv_i0;
  
      /* DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_B.DiscreteTimeIntegrator[2] = INS_DWork.DiscreteTimeIntegrator_DSTATE_o
        [2];
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' incorporates:
       *  Gain: '<S144>/Gain2'
       *  Sum: '<S144>/Add'
       */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_l[2] += (INS_P.Gain2_Gain_a *
        rtb_Multiply_j[2] + INS_B.DiscreteTimeIntegrator[2]) *
        INS_P.DiscreteTimeIntegrator1_gainval;
  
      /* Update for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' incorporates:
       *  DiscreteIntegrator: '<S144>/Discrete-Time Integrator1'
       *  Gain: '<S144>/Gain2'
       *  Gain: '<S144>/Gain3'
       */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[2] += INS_P.Gain3_Gain_p *
        rtb_Multiply_j[2] * INS_P.DiscreteTimeIntegrator_gainv_i0;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_c = (int8_T)
        rtb_FixPtRelationalOperator_l;
    } else {
      if (INS_DWork.GPS_Acc_Observer_MODE) {
        INS_DWork.GPS_Acc_Observer_MODE = false;
      }
    }
  
    /* End of Outputs for SubSystem: '<S123>/GPS_Acc_Observer' */
  
    /* Outputs for Enabled SubSystem: '<S123>/OpticalFlow_Acc_Observer' incorporates:
     *  EnablePort: '<S142>/Enable'
     */
    if (rtb_Compare_cl > 0) {
      if (!INS_DWork.OpticalFlow_Acc_Observer_MODE) {
        /* InitializeConditions for DiscreteIntegrator: '<S145>/Discrete-Time Integrator1' */
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[0] =
          INS_P.DiscreteTimeIntegrator1_IC;
        INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[1] =
          INS_P.DiscreteTimeIntegrator1_IC;
  
        /* InitializeConditions for DiscreteIntegrator: '<S145>/Discrete-Time Integrator' */
        INS_DWork.DiscreteTimeIntegrator_IC_LOA_k = 1U;
        INS_DWork.OpticalFlow_Acc_Observer_MODE = true;
      }
  
      /* Sum: '<S145>/Subtract' incorporates:
       *  DiscreteIntegrator: '<S145>/Discrete-Time Integrator1'
       */
      rtb_Square_l_idx_0 = rtb_uT[0] - INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[0];
      rtb_Square_l_idx_1 = rtb_uT[1] - INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[1];
  
      /* DiscreteIntegrator: '<S145>/Discrete-Time Integrator' */
      if (INS_DWork.DiscreteTimeIntegrator_IC_LOA_k != 0) {
        INS_DWork.DiscreteTimeIntegrator_DSTATE_j[0] = rtb_uT[0];
        INS_DWork.DiscreteTimeIntegrator_DSTATE_j[1] = rtb_uT[1];
      }
  
      /* Reshape: '<S142>/Reshape' incorporates:
       *  Constant: '<S142>/Constant'
       */
      INS_B.Reshape[2] = INS_P.Constant_Value_gi;
  
      /* Update for DiscreteIntegrator: '<S145>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_IC_LOA_k = 0U;
  
      /* Reshape: '<S142>/Reshape' incorporates:
       *  DiscreteIntegrator: '<S145>/Discrete-Time Integrator'
       */
      INS_B.Reshape[0] = INS_DWork.DiscreteTimeIntegrator_DSTATE_j[0];
  
      /* Update for DiscreteIntegrator: '<S145>/Discrete-Time Integrator1' incorporates:
       *  DiscreteIntegrator: '<S145>/Discrete-Time Integrator'
       *  Gain: '<S145>/Gain2'
       *  Sum: '<S145>/Add'
       */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[0] += (INS_P.Gain2_Gain_b *
        rtb_Square_l_idx_0 + INS_DWork.DiscreteTimeIntegrator_DSTATE_j[0]) *
        INS_P.DiscreteTimeIntegrator1_gainv_m;
  
      /* Update for DiscreteIntegrator: '<S145>/Discrete-Time Integrator' incorporates:
       *  Gain: '<S145>/Gain2'
       *  Gain: '<S145>/Gain3'
       */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_j[0] += INS_P.Gain3_Gain_i *
        rtb_Square_l_idx_0 * INS_P.DiscreteTimeIntegrator_gainva_l;
  
      /* Reshape: '<S142>/Reshape' incorporates:
       *  DiscreteIntegrator: '<S145>/Discrete-Time Integrator'
       */
      INS_B.Reshape[1] = INS_DWork.DiscreteTimeIntegrator_DSTATE_j[1];
  
      /* Update for DiscreteIntegrator: '<S145>/Discrete-Time Integrator1' incorporates:
       *  DiscreteIntegrator: '<S145>/Discrete-Time Integrator'
       *  Gain: '<S145>/Gain2'
       *  Sum: '<S145>/Add'
       */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[1] += (INS_P.Gain2_Gain_b *
        rtb_Square_l_idx_1 + INS_DWork.DiscreteTimeIntegrator_DSTATE_j[1]) *
        INS_P.DiscreteTimeIntegrator1_gainv_m;
  
      /* Update for DiscreteIntegrator: '<S145>/Discrete-Time Integrator' incorporates:
       *  Gain: '<S145>/Gain2'
       *  Gain: '<S145>/Gain3'
       */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_j[1] += INS_P.Gain3_Gain_i *
        rtb_Square_l_idx_1 * INS_P.DiscreteTimeIntegrator_gainva_l;
    } else {
      if (INS_DWork.OpticalFlow_Acc_Observer_MODE) {
        INS_DWork.OpticalFlow_Acc_Observer_MODE = false;
      }
    }
  
    /* End of Outputs for SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
  
    /* Sum: '<S143>/Sum2' incorporates:
     *  Constant: '<S143>/gravity'
     *  Product: '<S143>/Multiply'
     *  Product: '<S143>/Multiply1'
     *  Sum: '<S143>/Sum'
     */
    rtb_DiscreteTimeIntegrator_c_id = ((real32_T)rtb_Compare_cl * INS_B.Reshape[0]
      + (rtb_Delay_b ? INS_B.DiscreteTimeIntegrator[0] : 0.0F)) +
      INS_P.gravity_Value[0];
    rtb_DiscreteTimeIntegrator_c__0 = ((real32_T)rtb_Compare_cl * INS_B.Reshape[1]
      + (rtb_Delay_b ? INS_B.DiscreteTimeIntegrator[1] : 0.0F)) +
      INS_P.gravity_Value[1];
    rtb_DiscreteTimeIntegrator_c__1 = ((real32_T)rtb_Compare_cl * INS_B.Reshape[2]
      + (rtb_Delay_b ? INS_B.DiscreteTimeIntegrator[2] : 0.0F)) +
      INS_P.gravity_Value[2];
  
    /* Sum: '<S129>/Sum' incorporates:
     *  Constant: '<S124>/Constant'
     *  Constant: '<S124>/Constant1'
     *  Product: '<S130>/Multiply'
     *  Product: '<S131>/Multiply'
     */
    u0 = rtb_Delay_m_idx_1 * INS_P.Constant1_Value_cx - INS_P.Constant_Value_m *
      rtb_DiscreteTimeIntegrator_c__0;
  
    /* DeadZone: '<S124>/Dead Zone2' */
    if (u0 > INS_P.DeadZone2_End) {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[0] = u0 - INS_P.DeadZone2_End;
    } else if (u0 >= INS_P.DeadZone2_Start) {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[0] = 0.0F;
    } else {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[0] = u0 - INS_P.DeadZone2_Start;
    }
  
    /* Sum: '<S129>/Sum' incorporates:
     *  Constant: '<S124>/Constant'
     *  Constant: '<S124>/Constant1'
     *  Product: '<S130>/Multiply1'
     *  Product: '<S131>/Multiply1'
     */
    u0 = INS_P.Constant_Value_m * rtb_DiscreteTimeIntegrator_c_id -
      rtb_Inclination * INS_P.Constant1_Value_cx;
  
    /* DeadZone: '<S124>/Dead Zone2' */
    if (u0 > INS_P.DeadZone2_End) {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[1] = u0 - INS_P.DeadZone2_End;
    } else if (u0 >= INS_P.DeadZone2_Start) {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[1] = 0.0F;
    } else {
      /* Sum: '<S129>/Sum' */
      rtb_Divide1_m[1] = u0 - INS_P.DeadZone2_Start;
    }
  
    /* Sum: '<S129>/Sum' incorporates:
     *  Product: '<S130>/Multiply2'
     *  Product: '<S131>/Multiply2'
     */
    u0 = rtb_Inclination * rtb_DiscreteTimeIntegrator_c__0 - rtb_Delay_m_idx_1 *
      rtb_DiscreteTimeIntegrator_c_id;
  
    /* DeadZone: '<S124>/Dead Zone2' */
    if (u0 > INS_P.DeadZone2_End) {
      u0 -= INS_P.DeadZone2_End;
    } else if (u0 >= INS_P.DeadZone2_Start) {
      u0 = 0.0F;
    } else {
      u0 -= INS_P.DeadZone2_Start;
    }
  
    /* DotProduct: '<S127>/Dot Product' incorporates:
     *  Sum: '<S134>/Sum of Elements'
     */
    rtb_TrigonometricFunction1 = rtb_Inclination * rtb_Inclination +
      rtb_Delay_m_idx_1 * rtb_Delay_m_idx_1;
  
    /* Sqrt: '<S127>/Sqrt' incorporates:
     *  DotProduct: '<S127>/Dot Product'
     */
    rtb_DiscreteTimeIntegrator_df = sqrtf(rtb_TrigonometricFunction1);
  
    /* Sqrt: '<S126>/Sqrt' incorporates:
     *  DotProduct: '<S126>/Dot Product'
     */
    rtb_Square_l_idx_1 = sqrtf(rtb_DiscreteTimeIntegrator_c_id *
      rtb_DiscreteTimeIntegrator_c_id + rtb_DiscreteTimeIntegrator_c__0 *
      rtb_DiscreteTimeIntegrator_c__0);
  
    /* MinMax: '<S128>/Max1' incorporates:
     *  Constant: '<S128>/Constant'
     *  MinMax: '<S128>/Max'
     */
    rtb_Add_m = fmaxf(INS_P.Constant_Value_ew, fmaxf(rtb_DiscreteTimeIntegrator_df,
      rtb_Square_l_idx_1));
  
    /* Sum: '<S128>/Subtract' */
    rtb_DiscreteTimeIntegrator_df -= rtb_Square_l_idx_1;
  
    /* Abs: '<S128>/Abs' */
    rtb_DiscreteTimeIntegrator_df = fabsf(rtb_DiscreteTimeIntegrator_df);
  
    /* Sum: '<S128>/Subtract1' incorporates:
     *  Constant: '<S128>/Constant1'
     *  Gain: '<S128>/Gain'
     *  Product: '<S128>/Divide'
     */
    rtb_DiscreteTimeIntegrator_df = INS_P.Constant1_Value_o - 1.0F / rtb_Add_m *
      (INS_P.Gain_Gain_a * rtb_DiscreteTimeIntegrator_df);
  
    /* Saturate: '<S128>/Saturation' */
    if (rtb_DiscreteTimeIntegrator_df > INS_P.Saturation_UpperSat_g) {
      rtb_Add_m = INS_P.Saturation_UpperSat_g;
    } else if (rtb_DiscreteTimeIntegrator_df < INS_P.Saturation_LowerSat_n) {
      rtb_Add_m = INS_P.Saturation_LowerSat_n;
    } else {
      rtb_Add_m = rtb_DiscreteTimeIntegrator_df;
    }
  
    /* End of Saturate: '<S128>/Saturation' */
  
    /* DiscreteFir: '<S124>/Discrete FIR Filter' */
    rtb_DiscreteTimeIntegrator_df = rtb_Add_m *
      INS_P.DiscreteFIRFilter_Coefficients[0];
    for (rtb_Saturation1 = INS_DWork.DiscreteFIRFilter_circBuf; rtb_Saturation1 <
         29; rtb_Saturation1++) {
      rtb_DiscreteTimeIntegrator_df +=
        INS_DWork.DiscreteFIRFilter_states[rtb_Saturation1] *
        INS_P.DiscreteFIRFilter_Coefficients[i];
      i++;
    }
  
    for (rtb_Saturation1 = 0; rtb_Saturation1 <
         INS_DWork.DiscreteFIRFilter_circBuf; rtb_Saturation1++) {
      rtb_DiscreteTimeIntegrator_df +=
        INS_DWork.DiscreteFIRFilter_states[rtb_Saturation1] *
        INS_P.DiscreteFIRFilter_Coefficients[i];
      i++;
    }
  
    /* End of DiscreteFir: '<S124>/Discrete FIR Filter' */
  
    /* Logic: '<S143>/Logical Operator' */
    rtb_FixPtRelationalOperator_l = (rtb_Delay_b || (rtb_Compare_cl != 0));
  
    /* Gain: '<S124>/Gain' */
    rtb_Square_l_idx_1 = INS_P.Gain_Gain_oh * rtb_Divide1_m[0];
  
    /* Saturate: '<S124>/Saturation' */
    if (rtb_Square_l_idx_1 > INS_P.Saturation_UpperSat_k) {
      rtb_Square_l_idx_1 = INS_P.Saturation_UpperSat_k;
    } else {
      if (rtb_Square_l_idx_1 < INS_P.Saturation_LowerSat_k) {
        rtb_Square_l_idx_1 = INS_P.Saturation_LowerSat_k;
      }
    }
  
    /* Product: '<S124>/Multiply1' */
    rtb_Square_l_idx_1 *= rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S124>/Multiply' */
    rtb_ba_O_mDs2[0] = rtb_FixPtRelationalOperator_l ? rtb_Square_l_idx_1 : 0.0F;
  
    /* Gain: '<S124>/Gain' */
    rtb_Square_l_idx_1 = INS_P.Gain_Gain_oh * rtb_Divide1_m[1];
  
    /* Saturate: '<S124>/Saturation' */
    if (rtb_Square_l_idx_1 > INS_P.Saturation_UpperSat_k) {
      rtb_Square_l_idx_1 = INS_P.Saturation_UpperSat_k;
    } else {
      if (rtb_Square_l_idx_1 < INS_P.Saturation_LowerSat_k) {
        rtb_Square_l_idx_1 = INS_P.Saturation_LowerSat_k;
      }
    }
  
    /* Product: '<S124>/Multiply1' */
    rtb_Square_l_idx_1 *= rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S124>/Multiply' */
    rtb_ba_O_mDs2[1] = rtb_FixPtRelationalOperator_l ? rtb_Square_l_idx_1 : 0.0F;
  
    /* Gain: '<S124>/Gain' */
    rtb_Square_l_idx_1 = INS_P.Gain_Gain_oh * u0;
  
    /* Saturate: '<S124>/Saturation' */
    if (rtb_Square_l_idx_1 > INS_P.Saturation_UpperSat_k) {
      rtb_Square_l_idx_1 = INS_P.Saturation_UpperSat_k;
    } else {
      if (rtb_Square_l_idx_1 < INS_P.Saturation_LowerSat_k) {
        rtb_Square_l_idx_1 = INS_P.Saturation_LowerSat_k;
      }
    }
  
    /* Product: '<S124>/Multiply1' */
    rtb_Square_l_idx_1 *= rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S124>/Multiply' */
    rtb_ba_O_mDs2[2] = rtb_FixPtRelationalOperator_l ? rtb_Square_l_idx_1 : 0.0F;
  
    /* Sum: '<S134>/Sum of Elements' incorporates:
     *  Math: '<S134>/Math Function'
     */
    rtb_DiscreteTimeIntegrator_df = rtb_TrigonometricFunction1 + rtb_Delay_m_idx_2
      * rtb_Delay_m_idx_2;
  
    /* Math: '<S134>/Math Function1'
     *
     * About '<S134>/Math Function1':
     *  Operator: sqrt
     */
    if (rtb_DiscreteTimeIntegrator_df < 0.0F) {
      rtb_DiscreteTimeIntegrator_df = -sqrtf(fabsf(rtb_DiscreteTimeIntegrator_df));
    } else {
      rtb_DiscreteTimeIntegrator_df = sqrtf(rtb_DiscreteTimeIntegrator_df);
    }
  
    /* End of Math: '<S134>/Math Function1' */
  
    /* Switch: '<S134>/Switch' incorporates:
     *  Constant: '<S134>/Constant'
     *  Product: '<S134>/Product'
     */
    if (rtb_DiscreteTimeIntegrator_df <= INS_P.Switch_Threshold) {
      rtb_Inclination = 0.0F;
      rtb_Delay_m_idx_1 = 0.0F;
      rtb_Delay_m_idx_2 = 0.0F;
      rtb_DiscreteTimeIntegrator_df = INS_P.Constant_Value_f;
    }
  
    /* End of Switch: '<S134>/Switch' */
  
    /* Product: '<S134>/Divide1' */
    rtb_Multiply_j[0] = rtb_Inclination / rtb_DiscreteTimeIntegrator_df;
    rtb_Multiply_j[1] = rtb_Delay_m_idx_1 / rtb_DiscreteTimeIntegrator_df;
    rtb_Multiply_j[2] = rtb_Delay_m_idx_2 / rtb_DiscreteTimeIntegrator_df;
  
    /* Sum: '<S135>/Sum of Elements' incorporates:
     *  Math: '<S135>/Math Function'
     */
    rtb_DiscreteTimeIntegrator_df = (rtb_DiscreteTimeIntegrator_c_id *
      rtb_DiscreteTimeIntegrator_c_id + rtb_DiscreteTimeIntegrator_c__0 *
      rtb_DiscreteTimeIntegrator_c__0) + rtb_DiscreteTimeIntegrator_c__1 *
      rtb_DiscreteTimeIntegrator_c__1;
  
    /* Math: '<S135>/Math Function1'
     *
     * About '<S135>/Math Function1':
     *  Operator: sqrt
     */
    if (rtb_DiscreteTimeIntegrator_df < 0.0F) {
      rtb_DiscreteTimeIntegrator_df = -sqrtf(fabsf(rtb_DiscreteTimeIntegrator_df));
    } else {
      rtb_DiscreteTimeIntegrator_df = sqrtf(rtb_DiscreteTimeIntegrator_df);
    }
  
    /* End of Math: '<S135>/Math Function1' */
  
    /* Switch: '<S135>/Switch' incorporates:
     *  Constant: '<S135>/Constant'
     *  Product: '<S135>/Product'
     */
    if (rtb_DiscreteTimeIntegrator_df > INS_P.Switch_Threshold_b) {
      rtb_Inclination = rtb_DiscreteTimeIntegrator_c_id;
      rtb_Delay_m_idx_1 = rtb_DiscreteTimeIntegrator_c__0;
      rtb_Delay_m_idx_2 = rtb_DiscreteTimeIntegrator_c__1;
    } else {
      rtb_Inclination = 0.0F;
      rtb_Delay_m_idx_1 = 0.0F;
      rtb_Delay_m_idx_2 = 0.0F;
      rtb_DiscreteTimeIntegrator_df = INS_P.Constant_Value_a;
    }
  
    /* End of Switch: '<S135>/Switch' */
  
    /* Product: '<S135>/Divide1' */
    rtb_Divide1_m[0] = rtb_Inclination / rtb_DiscreteTimeIntegrator_df;
    rtb_Divide1_m[1] = rtb_Delay_m_idx_1 / rtb_DiscreteTimeIntegrator_df;
    rtb_Divide1_m[2] = rtb_Delay_m_idx_2 / rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S132>/Product4' */
    rtb_DiscreteTimeIntegrator_df = rtb_Multiply_j[0] * rtb_Divide1_m[1];
  
    /* Product: '<S132>/Product3' */
    rtb_Delay_m_idx_2 = rtb_Multiply_j[0];
  
    /* Product: '<S132>/Product5' */
    u0 = rtb_Multiply_j[1];
  
    /* SignalConversion: '<S137>/TmpSignal ConversionAtSquareInport1' incorporates:
     *  Product: '<S132>/Product'
     *  Product: '<S132>/Product1'
     *  Product: '<S132>/Product2'
     *  Product: '<S132>/Product3'
     *  Product: '<S132>/Product5'
     *  Sum: '<S132>/Subtract'
     *  Sum: '<S132>/Subtract1'
     *  Sum: '<S132>/Subtract2'
     */
    rtb_Multiply_j[0] = rtb_Multiply_j[1] * rtb_Divide1_m[2] - rtb_Multiply_j[2] *
      rtb_Divide1_m[1];
    rtb_Multiply_j[1] = rtb_Multiply_j[2] * rtb_Divide1_m[0] - rtb_Delay_m_idx_2 *
      rtb_Divide1_m[2];
    rtb_Multiply_j[2] = rtb_DiscreteTimeIntegrator_df - u0 * rtb_Divide1_m[0];
  
    /* Gain: '<S138>/Gain' incorporates:
     *  DiscreteIntegrator: '<S138>/Discrete-Time Integrator1'
     *  Sum: '<S138>/Sum'
     */
    rtb_MatrixConcatenate[0] = (rtb_MatrixConcatenate[0] -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[0]) * INS_P.Gain_Gain_l;
    rtb_MatrixConcatenate[1] = (rtb_MatrixConcatenate[1] -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[1]) * INS_P.Gain_Gain_l;
    rtb_DiscreteTimeIntegrator_c_id = (rtb_MatrixConcatenate[2] -
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[2]) * INS_P.Gain_Gain_l;
  
    /* Sqrt: '<S136>/Sqrt' incorporates:
     *  Math: '<S137>/Square'
     *  Sum: '<S137>/Sum of Elements'
     */
    rtb_DiscreteTimeIntegrator_df = sqrtf((rtb_Multiply_j[0] * rtb_Multiply_j[0] +
      rtb_Multiply_j[1] * rtb_Multiply_j[1]) + rtb_Multiply_j[2] * rtb_Multiply_j
      [2]);
  
    /* Product: '<S133>/Divide1' incorporates:
     *  Constant: '<S125>/Constant8'
     */
    u0 = rtb_DiscreteTimeIntegrator_df / INS_P.Constant8_Value;
  
    /* Saturate: '<S133>/Saturation' */
    if (u0 > INS_P.Saturation_UpperSat_e) {
      u0 = INS_P.Saturation_UpperSat_e;
    } else {
      if (u0 < INS_P.Saturation_LowerSat_f) {
        u0 = INS_P.Saturation_LowerSat_f;
      }
    }
  
    /* End of Saturate: '<S133>/Saturation' */
  
    /* Product: '<S133>/Divide2' incorporates:
     *  Constant: '<S125>/Constant8'
     *  Trigonometry: '<S133>/Trigonometric Function'
     */
    rtb_Inclination = arm_sin_f32(u0) * INS_P.Constant8_Value;
  
    /* MinMax: '<S133>/MinMax' incorporates:
     *  Constant: '<S133>/Constant1'
     */
    rtb_Delay_m_idx_1 = fmaxf(rtb_DiscreteTimeIntegrator_df,
      INS_P.Constant1_Value_nm);
  
    /* Relay: '<S146>/Relay1' */
    if (rtb_DiscreteTimeIntegrator_g >= INS_P.Relay1_OnVal_i) {
      INS_DWork.Relay1_Mode_l = true;
    } else {
      if (rtb_DiscreteTimeIntegrator_g <= INS_P.Relay1_OffVal_o) {
        INS_DWork.Relay1_Mode_l = false;
      }
    }
  
    if (INS_DWork.Relay1_Mode_l) {
      rtb_Delay_m_idx_2 = INS_P.Relay1_YOn;
    } else {
      rtb_Delay_m_idx_2 = INS_P.Relay1_YOff;
    }
  
    /* End of Relay: '<S146>/Relay1' */
  
    /* Logic: '<S146>/Logical Operator' incorporates:
     *  Logic: '<S120>/Logical Operator'
     */
    rtb_FixPtRelationalOperator_l = (((rtb_Relay2 != 0.0F) || rtb_Compare_cio) &&
      (rtb_Delay_m_idx_2 != 0.0F));
  
    /* Gain: '<S147>/mag_correct_gain' incorporates:
     *  Delay: '<S56>/Delay'
     *  Product: '<S147>/MX Product4'
     *  Selector: '<S147>/Selector4'
     */
    rtb_DiscreteTimeIntegrator_df = ((INS_DWork.Delay_8_DSTATE_l[1] *
      rtb_Product1_n[0] + INS_DWork.Delay_8_DSTATE_l[4] * rtb_Product1_n[1]) +
      INS_DWork.Delay_8_DSTATE_l[7] * rtb_Product1_n[2]) *
      INS_P.mag_correct_gain_Gain;
  
    /* Sum: '<S117>/Sum' incorporates:
     *  Delay: '<S56>/Delay'
     *  Gain: '<S125>/Gain'
     *  Product: '<S133>/Divide'
     *  Product: '<S147>/MX Product3'
     *  Selector: '<S147>/Selector2'
     *  Sum: '<S121>/Sum'
     */
    rtb_Delay_m_idx_2 = (rtb_Multiply_j[0] * rtb_Inclination / rtb_Delay_m_idx_1 *
                         INS_P.Gain_Gain_ok[0] + rtb_ba_O_mDs2[0]) +
      (rtb_FixPtRelationalOperator_l ? INS_DWork.Delay_8_DSTATE_l[2] *
       rtb_DiscreteTimeIntegrator_df : 0.0F);
    u0 = (rtb_Multiply_j[1] * rtb_Inclination / rtb_Delay_m_idx_1 *
          INS_P.Gain_Gain_ok[1] + rtb_ba_O_mDs2[1]) +
      (rtb_FixPtRelationalOperator_l ? INS_DWork.Delay_8_DSTATE_l[5] *
       rtb_DiscreteTimeIntegrator_df : 0.0F);
    rtb_DiscreteTimeIntegrator_df = (rtb_Multiply_j[2] * rtb_Inclination /
      rtb_Delay_m_idx_1 * INS_P.Gain_Gain_ok[2] + rtb_ba_O_mDs2[2]) +
      (rtb_FixPtRelationalOperator_l ? INS_DWork.Delay_8_DSTATE_l[8] *
       rtb_DiscreteTimeIntegrator_df : 0.0F);
    for (i = 0; i < 3; i++) {
      /* Math: '<S152>/Square' */
      rtb_ba_O_mDs2[i] = rtb_Switch3[i] * rtb_Switch3[i];
  
      /* Product: '<S117>/Multiply' incorporates:
       *  Delay: '<S56>/Delay'
       */
      rtb_Divide1_m[i] = INS_DWork.Delay_9_DSTATE[i + 6] *
        rtb_DiscreteTimeIntegrator_df + (INS_DWork.Delay_9_DSTATE[i + 3] * u0 +
        INS_DWork.Delay_9_DSTATE[i] * rtb_Delay_m_idx_2);
    }
  
    /* RelationalOperator: '<S149>/Compare' incorporates:
     *  Abs: '<S118>/Abs'
     *  Constant: '<S118>/Constant'
     *  Constant: '<S149>/Constant'
     *  Sqrt: '<S151>/Sqrt'
     *  Sum: '<S118>/Subtract'
     *  Sum: '<S152>/Sum of Elements'
     */
    rtb_FixPtRelationalOperator_l = (fabsf(sqrtf((rtb_ba_O_mDs2[0] +
      rtb_ba_O_mDs2[1]) + rtb_ba_O_mDs2[2]) - INS_P.Constant_Value_n2) <=
      INS_P.CompareToConstant_const_n);
  
    /* Gain: '<S150>/Gain' incorporates:
     *  Logic: '<S150>/Logical Operator'
     */
    rtb_DiscreteTimeIntegrator_df = rtb_Relay2 == 0.0F ? (real32_T)
      INS_P.Gain_Gain_g * 0.015625F : 0.0F;
  
    /* RateLimiter: '<S150>/Rate Limiter' */
    rtb_Inclination = rtb_DiscreteTimeIntegrator_df - INS_DWork.PrevY;
    if (rtb_Inclination > INS_P.RateLimiter_RisingLim) {
      rtb_DiscreteTimeIntegrator_df = INS_DWork.PrevY +
        INS_P.RateLimiter_RisingLim;
    } else {
      if (rtb_Inclination < INS_P.RateLimiter_FallingLim) {
        rtb_DiscreteTimeIntegrator_df = INS_DWork.PrevY +
          INS_P.RateLimiter_FallingLim;
      }
    }
  
    INS_DWork.PrevY = rtb_DiscreteTimeIntegrator_df;
  
    /* End of RateLimiter: '<S150>/Rate Limiter' */
  
    /* Saturate: '<S150>/Saturation' */
    if (rtb_DiscreteTimeIntegrator_df > INS_P.Saturation_UpperSat_ky) {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation_UpperSat_ky;
    } else {
      if (rtb_DiscreteTimeIntegrator_df < INS_P.Saturation_LowerSat_f3) {
        rtb_DiscreteTimeIntegrator_df = INS_P.Saturation_LowerSat_f3;
      }
    }
  
    /* End of Saturate: '<S150>/Saturation' */
  
    /* Update for DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' */
    INS_DWork.DiscreteTimeIntegrator1_IC_LO_o = 0U;
  
    /* Gain: '<S118>/Gain3' incorporates:
     *  DataTypeConversion: '<S118>/Data Type Conversion'
     *  Product: '<S118>/MX Product1'
     */
    u0 = rtb_Divide1_m[0] * (real32_T)rtb_FixPtRelationalOperator_l *
      INS_P.Gain3_Gain_k;
  
    /* Saturate: '<S118>/Saturation2' */
    if (u0 > INS_P.Saturation2_UpperSat_h) {
      u0 = INS_P.Saturation2_UpperSat_h;
    } else {
      if (u0 < INS_P.Saturation2_LowerSat_o) {
        u0 = INS_P.Saturation2_LowerSat_o;
      }
    }
  
    /* Saturate: '<S150>/Saturation1' incorporates:
     *  Delay: '<S56>/Delay'
     */
    if (INS_DWork.Delay_10_DSTATE[0] > INS_P.Saturation1_UpperSat_p) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_UpperSat_p;
    } else if (INS_DWork.Delay_10_DSTATE[0] < INS_P.Saturation1_LowerSat_j) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_LowerSat_j;
    } else {
      rtb_Delay_m_idx_2 = INS_DWork.Delay_10_DSTATE[0];
    }
  
    /* Sum: '<S118>/Sum2' incorporates:
     *  Gain: '<S118>/Gain4'
     *  Product: '<S150>/Product'
     */
    rtb_Multiply_j[0] = u0 - rtb_Delay_m_idx_2 * rtb_Relay2 *
      rtb_DiscreteTimeIntegrator_df * INS_P.Gain4_Gain_d;
  
    /* Update for DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' incorporates:
     *  DiscreteIntegrator: '<S138>/Discrete-Time Integrator'
     */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[0] +=
      INS_P.DiscreteTimeIntegrator1_gainv_d *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[0];
  
    /* Gain: '<S118>/Gain3' incorporates:
     *  DataTypeConversion: '<S118>/Data Type Conversion'
     *  Product: '<S118>/MX Product1'
     */
    u0 = rtb_Divide1_m[1] * (real32_T)rtb_FixPtRelationalOperator_l *
      INS_P.Gain3_Gain_k;
  
    /* Saturate: '<S118>/Saturation2' */
    if (u0 > INS_P.Saturation2_UpperSat_h) {
      u0 = INS_P.Saturation2_UpperSat_h;
    } else {
      if (u0 < INS_P.Saturation2_LowerSat_o) {
        u0 = INS_P.Saturation2_LowerSat_o;
      }
    }
  
    /* Saturate: '<S150>/Saturation1' incorporates:
     *  Delay: '<S56>/Delay'
     */
    if (INS_DWork.Delay_10_DSTATE[1] > INS_P.Saturation1_UpperSat_p) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_UpperSat_p;
    } else if (INS_DWork.Delay_10_DSTATE[1] < INS_P.Saturation1_LowerSat_j) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_LowerSat_j;
    } else {
      rtb_Delay_m_idx_2 = INS_DWork.Delay_10_DSTATE[1];
    }
  
    /* Sum: '<S118>/Sum2' incorporates:
     *  Gain: '<S118>/Gain4'
     *  Product: '<S150>/Product'
     */
    rtb_Multiply_j[1] = u0 - rtb_Delay_m_idx_2 * rtb_Relay2 *
      rtb_DiscreteTimeIntegrator_df * INS_P.Gain4_Gain_d;
  
    /* Update for DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' incorporates:
     *  DiscreteIntegrator: '<S138>/Discrete-Time Integrator'
     */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[1] +=
      INS_P.DiscreteTimeIntegrator1_gainv_d *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[1];
  
    /* Gain: '<S118>/Gain3' incorporates:
     *  DataTypeConversion: '<S118>/Data Type Conversion'
     *  Product: '<S118>/MX Product1'
     */
    u0 = rtb_Divide1_m[2] * (real32_T)rtb_FixPtRelationalOperator_l *
      INS_P.Gain3_Gain_k;
  
    /* Saturate: '<S118>/Saturation2' */
    if (u0 > INS_P.Saturation2_UpperSat_h) {
      u0 = INS_P.Saturation2_UpperSat_h;
    } else {
      if (u0 < INS_P.Saturation2_LowerSat_o) {
        u0 = INS_P.Saturation2_LowerSat_o;
      }
    }
  
    /* Saturate: '<S150>/Saturation1' incorporates:
     *  Delay: '<S56>/Delay'
     */
    if (INS_DWork.Delay_10_DSTATE[2] > INS_P.Saturation1_UpperSat_p) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_UpperSat_p;
    } else if (INS_DWork.Delay_10_DSTATE[2] < INS_P.Saturation1_LowerSat_j) {
      rtb_Delay_m_idx_2 = INS_P.Saturation1_LowerSat_j;
    } else {
      rtb_Delay_m_idx_2 = INS_DWork.Delay_10_DSTATE[2];
    }
  
    /* Sum: '<S118>/Sum2' incorporates:
     *  Gain: '<S118>/Gain4'
     *  Product: '<S150>/Product'
     */
    rtb_Multiply_j[2] = u0 - rtb_Delay_m_idx_2 * rtb_Relay2 *
      rtb_DiscreteTimeIntegrator_df * INS_P.Gain4_Gain_d;
  
    /* Update for DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' incorporates:
     *  DiscreteIntegrator: '<S138>/Discrete-Time Integrator'
     */
    INS_DWork.DiscreteTimeIntegrator1_DSTAT_n[2] +=
      INS_P.DiscreteTimeIntegrator1_gainv_d *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[2];
  
    /* Update for Delay: '<S122>/Delay' */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 199; rtb_Saturation1++) {
      i = (rtb_Saturation1 + 1) * 3;
      INS_DWork.Delay_DSTATE_lv[rtb_Saturation1 * 3] = INS_DWork.Delay_DSTATE_lv[i];
      INS_DWork.Delay_DSTATE_lv[rtb_Saturation1 * 3 + 1] =
        INS_DWork.Delay_DSTATE_lv[i + 1];
      INS_DWork.Delay_DSTATE_lv[rtb_Saturation1 * 3 + 2] =
        INS_DWork.Delay_DSTATE_lv[i + 2];
    }
  
    INS_DWork.Delay_DSTATE_lv[597] = rtb_DiscreteTimeIntegrator_h[0];
    INS_DWork.Delay_DSTATE_lv[598] = rtb_DiscreteTimeIntegrator_h[1];
    INS_DWork.Delay_DSTATE_lv[599] = rtb_DiscreteTimeIntegrator_h[2];
  
    /* End of Update for Delay: '<S122>/Delay' */
  
    /* Update for UnitDelay: '<S139>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_ff = rtb_Delay_b;
  
    /* Update for DiscreteFir: '<S124>/Discrete FIR Filter' */
    /* Update circular buffer index */
    INS_DWork.DiscreteFIRFilter_circBuf--;
    if (INS_DWork.DiscreteFIRFilter_circBuf < 0) {
      INS_DWork.DiscreteFIRFilter_circBuf = 28;
    }
  
    /* Update circular buffer */
    INS_DWork.DiscreteFIRFilter_states[INS_DWork.DiscreteFIRFilter_circBuf] =
      rtb_Add_m;
  
    /* End of Update for DiscreteFir: '<S124>/Discrete FIR Filter' */
  
    /* Update for DiscreteIntegrator: '<S138>/Discrete-Time Integrator' incorporates:
     *  Gain: '<S138>/Gain1'
     *  Sum: '<S138>/Add'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_g[0] += (INS_P.Gain1_Gain_m *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[0] + rtb_MatrixConcatenate[0]) *
      INS_P.DiscreteTimeIntegrator_gainva_o;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_g[1] += (INS_P.Gain1_Gain_m *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[1] + rtb_MatrixConcatenate[1]) *
      INS_P.DiscreteTimeIntegrator_gainva_o;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_g[2] += (INS_P.Gain1_Gain_m *
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[2] +
      rtb_DiscreteTimeIntegrator_c_id) * INS_P.DiscreteTimeIntegrator_gainva_o;
  
    /* End of Outputs for SubSystem: '<S56>/Correct' */
  
    /* Outputs for Atomic SubSystem: '<S56>/Integrate' */
    /* Switch: '<S153>/Switch' incorporates:
     *  Constant: '<S153>/Constant'
     *  Delay: '<S153>/Delay'
     *  RelationalOperator: '<S157>/FixPt Relational Operator'
     *  UnitDelay: '<S157>/Delay Input1'
     */
    if ((int32_T)rtb_heading_quality > (int32_T)INS_DWork.DelayInput1_DSTATE_dy) {
      rtb_Compare_cl = INS_P.Constant_Value_mm;
    } else {
      rtb_Compare_cl = INS_DWork.Delay_DSTATE_f0;
    }
  
    /* End of Switch: '<S153>/Switch' */
  
    /* DiscreteIntegrator: '<S154>/Discrete-Time Integrator' */
    if (INS_DWork.DiscreteTimeIntegrator_IC_LOADI != 0) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] =
        INS_B.OutportBufferForquaternion_0[0];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1] =
        INS_B.OutportBufferForquaternion_0[1];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2] =
        INS_B.OutportBufferForquaternion_0[2];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3] =
        INS_B.OutportBufferForquaternion_0[3];
    }
  
    if (rtb_heading_quality || (INS_DWork.DiscreteTimeIntegrator_PrevRese != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] =
        INS_B.OutportBufferForquaternion_0[0];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1] =
        INS_B.OutportBufferForquaternion_0[1];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2] =
        INS_B.OutportBufferForquaternion_0[2];
      INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3] =
        INS_B.OutportBufferForquaternion_0[3];
    }
  
    /* Sqrt: '<S161>/sqrt' incorporates:
     *  DiscreteIntegrator: '<S154>/Discrete-Time Integrator'
     *  Product: '<S162>/Product'
     *  Product: '<S162>/Product1'
     *  Product: '<S162>/Product2'
     *  Product: '<S162>/Product3'
     *  Sum: '<S162>/Sum'
     */
    rtb_DiscreteTimeIntegrator_df = sqrtf
      (((INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] *
         INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] +
         INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1] *
         INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1]) +
        INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2] *
        INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2]) +
       INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3] *
       INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3]);
  
    /* Product: '<S160>/Product' incorporates:
     *  DiscreteIntegrator: '<S154>/Discrete-Time Integrator'
     */
    rtb_Add_m = INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] /
      rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S160>/Product1' incorporates:
     *  DiscreteIntegrator: '<S154>/Discrete-Time Integrator'
     */
    rtb_Inclination = INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1] /
      rtb_DiscreteTimeIntegrator_df;
  
    /* Sum: '<S155>/Sum1' incorporates:
     *  DiscreteIntegrator: '<S155>/Discrete-Time Integrator2'
     */
    rtb_DiscreteTimeIntegrator_c_id = INS_DWork.DiscreteTimeIntegrator2_DSTATE[0]
      + rtb_VectorConcatenate_idx_0;
  
    /* Sum: '<S60>/Sum2' */
    rtb_ba_O_mDs2[0] = rtb_Divide1_m[0] + rtb_DiscreteTimeIntegrator_c_id;
  
    /* Sum: '<S155>/Sum1' incorporates:
     *  DiscreteIntegrator: '<S155>/Discrete-Time Integrator2'
     */
    rtb_VectorConcatenate_idx_0 = rtb_DiscreteTimeIntegrator_c_id;
    rtb_DiscreteTimeIntegrator_c_id = INS_DWork.DiscreteTimeIntegrator2_DSTATE[1]
      + rtb_VectorConcatenate_idx_1;
  
    /* Sum: '<S60>/Sum2' */
    rtb_ba_O_mDs2[1] = rtb_Divide1_m[1] + rtb_DiscreteTimeIntegrator_c_id;
  
    /* Sum: '<S155>/Sum1' incorporates:
     *  DiscreteIntegrator: '<S155>/Discrete-Time Integrator2'
     */
    rtb_VectorConcatenate_idx_1 = rtb_DiscreteTimeIntegrator_c_id;
    rtb_DiscreteTimeIntegrator_c_id = INS_DWork.DiscreteTimeIntegrator2_DSTATE[2]
      + rtb_Product3_eb;
  
    /* Sum: '<S60>/Sum2' */
    rtb_ba_O_mDs2[2] = rtb_Divide1_m[2] + rtb_DiscreteTimeIntegrator_c_id;
  
    /* Product: '<S160>/Product2' incorporates:
     *  DiscreteIntegrator: '<S154>/Discrete-Time Integrator'
     */
    rtb_Delay_m_idx_1 = INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2] /
      rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S160>/Product3' incorporates:
     *  DiscreteIntegrator: '<S154>/Discrete-Time Integrator'
     */
    rtb_Product3_eb = INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3] /
      rtb_DiscreteTimeIntegrator_df;
  
    /* Product: '<S159>/Product' incorporates:
     *  Product: '<S165>/Product'
     */
    rtb_Delay_m_idx_2 = rtb_Add_m * rtb_Add_m;
    rtb_DiscreteTimeIntegrator_c__0 = rtb_Inclination * rtb_Inclination;
    u0 = rtb_Delay_m_idx_1 * rtb_Delay_m_idx_1;
    rtb_Square_l_idx_1 = rtb_Product3_eb * rtb_Product3_eb;
  
    /* Sum: '<S159>/Add' incorporates:
     *  Product: '<S159>/Product'
     *  Sum: '<S165>/Add'
     */
    rtb_DiscreteTimeIntegrator_df = ((rtb_Delay_m_idx_2 +
      rtb_DiscreteTimeIntegrator_c__0) - u0) - rtb_Square_l_idx_1;
    rtb_VectorConcatenate_m[0] = rtb_DiscreteTimeIntegrator_df;
  
    /* Sum: '<S159>/Add5' incorporates:
     *  Product: '<S159>/Product'
     *  Sum: '<S159>/Add8'
     */
    rtb_Delay_m_idx_2 -= rtb_DiscreteTimeIntegrator_c__0;
    rtb_VectorConcatenate_m[4] = (rtb_Delay_m_idx_2 + u0) - rtb_Square_l_idx_1;
  
    /* Sum: '<S159>/Add8' incorporates:
     *  Product: '<S159>/Product'
     */
    rtb_VectorConcatenate_m[8] = (rtb_Delay_m_idx_2 - u0) + rtb_Square_l_idx_1;
  
    /* Product: '<S159>/Product1' incorporates:
     *  Product: '<S159>/Product5'
     */
    rtb_Delay_m_idx_2 = rtb_Inclination * rtb_Delay_m_idx_1;
  
    /* Product: '<S159>/Product2' incorporates:
     *  Product: '<S159>/Product6'
     */
    u0 = rtb_Add_m * rtb_Product3_eb;
  
    /* Sum: '<S159>/Add1' incorporates:
     *  Product: '<S159>/Product1'
     *  Product: '<S159>/Product2'
     *  Sum: '<S165>/Add1'
     */
    rtb_Square_l_idx_1 = rtb_Delay_m_idx_2 + u0;
  
    /* Gain: '<S159>/Gain' incorporates:
     *  Sum: '<S159>/Add1'
     */
    rtb_VectorConcatenate_m[1] = rtb_Square_l_idx_1 * INS_P.Gain_Gain_f;
  
    /* Product: '<S159>/Product3' incorporates:
     *  Product: '<S159>/Product9'
     */
    rtb_DiscreteTimeIntegrator_c__0 = rtb_Inclination * rtb_Product3_eb;
  
    /* Product: '<S159>/Product4' incorporates:
     *  Product: '<S159>/Product10'
     */
    rtb_DiscreteTimeIntegrator_c__1 = rtb_Add_m * rtb_Delay_m_idx_1;
  
    /* Gain: '<S159>/Gain1' incorporates:
     *  Product: '<S159>/Product3'
     *  Product: '<S159>/Product4'
     *  Sum: '<S159>/Add2'
     */
    rtb_VectorConcatenate_m[2] = (rtb_DiscreteTimeIntegrator_c__0 -
      rtb_DiscreteTimeIntegrator_c__1) * INS_P.Gain1_Gain_k;
  
    /* Gain: '<S159>/Gain2' incorporates:
     *  Sum: '<S159>/Add3'
     */
    rtb_VectorConcatenate_m[3] = (rtb_Delay_m_idx_2 - u0) * INS_P.Gain2_Gain_oj;
  
    /* Product: '<S159>/Product7' incorporates:
     *  Product: '<S159>/Product12'
     */
    rtb_Delay_m_idx_2 = rtb_Delay_m_idx_1 * rtb_Product3_eb;
  
    /* Product: '<S159>/Product8' incorporates:
     *  Product: '<S159>/Product11'
     */
    u0 = rtb_Add_m * rtb_Inclination;
  
    /* Gain: '<S159>/Gain3' incorporates:
     *  Product: '<S159>/Product7'
     *  Product: '<S159>/Product8'
     *  Sum: '<S159>/Add4'
     */
    rtb_VectorConcatenate_m[5] = (rtb_Delay_m_idx_2 + u0) * INS_P.Gain3_Gain_dx;
  
    /* Gain: '<S159>/Gain4' incorporates:
     *  Sum: '<S159>/Add6'
     */
    rtb_VectorConcatenate_m[6] = (rtb_DiscreteTimeIntegrator_c__0 +
      rtb_DiscreteTimeIntegrator_c__1) * INS_P.Gain4_Gain_o;
  
    /* Gain: '<S159>/Gain5' incorporates:
     *  Sum: '<S159>/Add7'
     */
    rtb_VectorConcatenate_m[7] = (rtb_Delay_m_idx_2 - u0) * INS_P.Gain5_Gain;
  
    /* Math: '<S154>/Transpose2' */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      rtb_Transpose2[3 * rtb_Saturation1] =
        rtb_VectorConcatenate_m[rtb_Saturation1];
      rtb_Transpose2[1 + 3 * rtb_Saturation1] =
        rtb_VectorConcatenate_m[rtb_Saturation1 + 3];
      rtb_Transpose2[2 + 3 * rtb_Saturation1] =
        rtb_VectorConcatenate_m[rtb_Saturation1 + 6];
    }
  
    /* End of Math: '<S154>/Transpose2' */
  
    /* Gain: '<S163>/Gain5' incorporates:
     *  Selector: '<S156>/Selector2'
     */
    u0 = INS_P.Gain5_Gain_d * rtb_Transpose2[6];
  
    /* Saturate: '<S163>/Saturation1' */
    if (u0 > INS_P.Saturation1_UpperSat_o) {
      u0 = INS_P.Saturation1_UpperSat_o;
    } else {
      if (u0 < INS_P.Saturation1_LowerSat_b) {
        u0 = INS_P.Saturation1_LowerSat_b;
      }
    }
  
    /* End of Saturate: '<S163>/Saturation1' */
  
    /* Trigonometry: '<S163>/Trigonometric Function' */
    if (u0 > 1.0F) {
      u0 = 1.0F;
    } else {
      if (u0 < -1.0F) {
        u0 = -1.0F;
      }
    }
  
    rtb_DiscreteTimeIntegrator_c__0 = asinf(u0);
  
    /* End of Trigonometry: '<S163>/Trigonometric Function' */
  
    /* Trigonometry: '<S163>/Trigonometric Function1' incorporates:
     *  Selector: '<S156>/Selector2'
     */
    rtb_DiscreteTimeIntegrator_c__1 = atan2f(rtb_Transpose2[7], rtb_Transpose2[8]);
  
    /* Trigonometry: '<S165>/Trigonometric Function1' incorporates:
     *  Gain: '<S165>/Gain'
     */
    rtb_TrigonometricFunction1 = atan2f(rtb_Square_l_idx_1 * INS_P.Gain_Gain_h,
      rtb_DiscreteTimeIntegrator_df);
  
    /* Trigonometry: '<S164>/TrigoFcn' */
    rtb_DiscreteTimeIntegrator_df = arm_sin_f32(rtb_TrigonometricFunction1);
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentate2In1' incorporates:
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[3] = rtb_DiscreteTimeIntegrator_df;
  
    /* Trigonometry: '<S164>/TrigoFcn1' */
    rtb_Square_l_idx_1 = arm_cos_f32(rtb_TrigonometricFunction1);
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentate2In2' incorporates:
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[4] = rtb_Square_l_idx_1;
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentate2In3' incorporates:
     *  Constant: '<S164>/Constant1'
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[5] = INS_P.Constant1_Value_g;
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentate3In1' incorporates:
     *  Constant: '<S164>/Constant1'
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[6] = INS_P.Constant1_Value_g;
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentate3In2' incorporates:
     *  Constant: '<S164>/Constant1'
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[7] = INS_P.Constant1_Value_g;
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentateIn1' incorporates:
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[0] = rtb_Square_l_idx_1;
  
    /* SignalConversion: '<S164>/ConcatBufferAtVecConcentateIn3' incorporates:
     *  Constant: '<S164>/Constant1'
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[2] = INS_P.Constant1_Value_g;
  
    /* Constant: '<S164>/Constant' incorporates:
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[8] = INS_P.Constant_Value_ev;
  
    /* Gain: '<S164>/Gain' incorporates:
     *  Delay: '<Root>/Delay'
     */
    INS_DWork.Delay_6_DSTATE[1] = INS_P.Gain_Gain_i *
      rtb_DiscreteTimeIntegrator_df;
  
    /* Update for Delay: '<S153>/Delay' */
    INS_DWork.Delay_DSTATE_f0 = rtb_Compare_cl;
  
    /* Update for UnitDelay: '<S157>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_dy = rtb_heading_quality;
  
    /* Update for DiscreteIntegrator: '<S154>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S154>/Constant'
     *  Gain: '<S154>/Gain2'
     *  Product: '<S158>/Product'
     *  Product: '<S158>/Product1'
     *  Product: '<S158>/Product10'
     *  Product: '<S158>/Product11'
     *  Product: '<S158>/Product12'
     *  Product: '<S158>/Product13'
     *  Product: '<S158>/Product14'
     *  Product: '<S158>/Product15'
     *  Product: '<S158>/Product2'
     *  Product: '<S158>/Product3'
     *  Product: '<S158>/Product4'
     *  Product: '<S158>/Product5'
     *  Product: '<S158>/Product6'
     *  Product: '<S158>/Product7'
     *  Product: '<S158>/Product8'
     *  Product: '<S158>/Product9'
     *  Sum: '<S158>/Add'
     *  Sum: '<S158>/Add1'
     *  Sum: '<S158>/Add2'
     *  Sum: '<S158>/Add3'
     */
    INS_DWork.DiscreteTimeIntegrator_IC_LOADI = 0U;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_k[0] += (((rtb_Add_m *
      INS_P.Constant_Value_p - rtb_Inclination * rtb_ba_O_mDs2[0]) -
      rtb_Delay_m_idx_1 * rtb_ba_O_mDs2[1]) - rtb_Product3_eb * rtb_ba_O_mDs2[2]) *
      INS_P.Gain2_Gain_i * INS_P.DiscreteTimeIntegrator_gainva_j;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_k[1] += (((rtb_Add_m * rtb_ba_O_mDs2[0]
      + rtb_Inclination * INS_P.Constant_Value_p) + rtb_Delay_m_idx_1 *
      rtb_ba_O_mDs2[2]) - rtb_Product3_eb * rtb_ba_O_mDs2[1]) * INS_P.Gain2_Gain_i
      * INS_P.DiscreteTimeIntegrator_gainva_j;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_k[2] += (((rtb_Add_m * rtb_ba_O_mDs2[1]
      - rtb_Inclination * rtb_ba_O_mDs2[2]) + rtb_Delay_m_idx_1 *
      INS_P.Constant_Value_p) + rtb_Product3_eb * rtb_ba_O_mDs2[0]) *
      INS_P.Gain2_Gain_i * INS_P.DiscreteTimeIntegrator_gainva_j;
    INS_DWork.DiscreteTimeIntegrator_DSTATE_k[3] += (((rtb_Add_m * rtb_ba_O_mDs2[2]
      + rtb_Inclination * rtb_ba_O_mDs2[1]) - rtb_Delay_m_idx_1 * rtb_ba_O_mDs2[0])
      + rtb_Product3_eb * INS_P.Constant_Value_p) * INS_P.Gain2_Gain_i *
      INS_P.DiscreteTimeIntegrator_gainva_j;
    INS_DWork.DiscreteTimeIntegrator_PrevRese = (int8_T)rtb_heading_quality;
  
    /* Update for DiscreteIntegrator: '<S155>/Discrete-Time Integrator2' */
    INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] +=
      INS_P.DiscreteTimeIntegrator2_gainval * rtb_Multiply_j[0];
    if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] >=
        INS_P.DiscreteTimeIntegrator2_UpperSa) {
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] =
        INS_P.DiscreteTimeIntegrator2_UpperSa;
    } else {
      if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] <=
          INS_P.DiscreteTimeIntegrator2_LowerSa) {
        INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] =
          INS_P.DiscreteTimeIntegrator2_LowerSa;
      }
    }
  
    INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] +=
      INS_P.DiscreteTimeIntegrator2_gainval * rtb_Multiply_j[1];
    if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] >=
        INS_P.DiscreteTimeIntegrator2_UpperSa) {
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] =
        INS_P.DiscreteTimeIntegrator2_UpperSa;
    } else {
      if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] <=
          INS_P.DiscreteTimeIntegrator2_LowerSa) {
        INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] =
          INS_P.DiscreteTimeIntegrator2_LowerSa;
      }
    }
  
    INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] +=
      INS_P.DiscreteTimeIntegrator2_gainval * rtb_Multiply_j[2];
    if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] >=
        INS_P.DiscreteTimeIntegrator2_UpperSa) {
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] =
        INS_P.DiscreteTimeIntegrator2_UpperSa;
    } else {
      if (INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] <=
          INS_P.DiscreteTimeIntegrator2_LowerSa) {
        INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] =
          INS_P.DiscreteTimeIntegrator2_LowerSa;
      }
    }
  
    /* End of Update for DiscreteIntegrator: '<S155>/Discrete-Time Integrator2' */
    /* End of Outputs for SubSystem: '<S56>/Integrate' */
  
    /* Outputs for Atomic SubSystem: '<S57>/Process' */
    /* Logic: '<S267>/Logical Operator' */
    rtb_GPS_vaid = !rtb_Compare_cio;
  
    /* Logic: '<S259>/Logical Operator' incorporates:
     *  Constant: '<S261>/Constant'
     *  RelationalOperator: '<S261>/Compare'
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_Delay_b = (rtb_GPS_vaid && (rtb_Sum4_k > INS_P.Constant_Value_be));
  
    /* RelationalOperator: '<S239>/Compare' incorporates:
     *  Constant: '<S239>/Constant'
     *  Logic: '<S259>/Logical Operator2'
     *  SignalConversion: '<S266>/TmpSignal ConversionAtSignal Conversion1Inport1'
     *  SignalConversion: '<S269>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_Compare_le = ((rtb_Delay_b || rtb_LogicalOperator_i ||
                       rtb_FixPtRelationalOperator_if) > (int32_T)
                      INS_P.Constant_Value_bt);
  
    /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
    /* Logic: '<S236>/Logical Operator2' incorporates:
     *  Inport: '<Root>/reset'
     *  RelationalOperator: '<S238>/FixPt Relational Operator'
     *  UnitDelay: '<S238>/Delay Input1'
     */
    rtb_LogicalOperator2_m = (((int32_T)rtb_Compare_le > (int32_T)
      INS_DWork.DelayInput1_DSTATE_j) || (INS_U.reset != 0));
  
    /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
    /* Logic: '<S258>/Logical Operator' incorporates:
     *  Constant: '<S260>/Constant'
     *  RelationalOperator: '<S260>/Compare'
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_FixPtRelationalOperator_l = (rtb_GPS_vaid && (rtb_Sum4_k >
      INS_P.Constant_Value_o0));
  
    /* RelationalOperator: '<S241>/Compare' incorporates:
     *  Constant: '<S241>/Constant'
     *  Logic: '<S258>/Logical Operator1'
     *  SignalConversion: '<S268>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_Compare_e = ((rtb_FixPtRelationalOperator_l || rtb_LogicalOperator3) >
                     (int32_T)INS_P.Constant_Value_gn);
  
    /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
    /* Logic: '<S237>/Logical Operator2' incorporates:
     *  Inport: '<Root>/reset'
     *  RelationalOperator: '<S240>/FixPt Relational Operator'
     *  UnitDelay: '<S240>/Delay Input1'
     */
    rtb_LogicalOperator2_o5 = (((int32_T)rtb_Compare_e > (int32_T)
      INS_DWork.DelayInput1_DSTATE_o) || (INS_U.reset != 0));
  
    /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
    /* RelationalOperator: '<S246>/Compare' incorporates:
     *  Constant: '<S246>/Constant'
     */
    rtb_Compare_ag = ((int32_T)rtb_Delay_b > (int32_T)INS_P.Constant_Value_m3);
  
    /* Switch: '<S243>/Switch4' incorporates:
     *  Delay: '<S243>/Delay1'
     *  Delay: '<S57>/Delay'
     *  Product: '<S243>/Divide'
     *  RelationalOperator: '<S245>/FixPt Relational Operator'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion2Inport1'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion5Inport1'
     *  Sum: '<S243>/Sum'
     *  UnitDelay: '<S245>/Delay Input1'
     */
    if ((int32_T)rtb_Compare_ag > (int32_T)INS_DWork.DelayInput1_DSTATE_dp) {
      rtb_Gain6 = rtb_Scalefactor3 - INS_DWork.Delay_11_DSTATE /
        INS_B.OutportBufferFordz_dh;
    } else {
      rtb_Gain6 = INS_DWork.Delay1_DSTATE;
    }
  
    /* End of Switch: '<S243>/Switch4' */
  
    /* RelationalOperator: '<S248>/Compare' incorporates:
     *  Constant: '<S248>/Constant'
     */
    rtb_Compare_os = ((int32_T)rtb_FixPtRelationalOperator_l > (int32_T)
                      INS_P.Constant_Value_pt);
  
    /* Switch: '<S244>/Switch4' incorporates:
     *  Delay: '<S244>/Delay'
     *  Delay: '<S244>/Delay1'
     *  Delay: '<S57>/Delay'
     *  Product: '<S244>/Divide'
     *  RelationalOperator: '<S247>/FixPt Relational Operator'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion1Inport1'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion3Inport1'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion4Inport1'
     *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal ConversionInport1'
     *  Sum: '<S244>/Sum'
     *  Switch: '<S244>/Switch5'
     *  UnitDelay: '<S247>/Delay Input1'
     */
    if ((int32_T)rtb_Compare_os > (int32_T)INS_DWork.DelayInput1_DSTATE_m) {
      rtb_WGS84_pos_ref_j_idx_0 = rtb_Scalefactor1 - INS_DWork.Delay_9_DSTATE_m /
        INS_B.Add;
      rtb_Min = rtb_Scalefactor2 - INS_DWork.Delay_10_DSTATE_a / INS_B.Product4;
      rtb_WGS84_derivatives_ref_idx_0 = INS_B.Add;
      rtb_WGS84_derivatives_ref_idx_1 = INS_B.Product4;
    } else {
      rtb_WGS84_pos_ref_j_idx_0 = INS_DWork.Delay_DSTATE[0];
      rtb_Min = INS_DWork.Delay_DSTATE[1];
      rtb_WGS84_derivatives_ref_idx_0 = INS_DWork.Delay1_DSTATE_o[0];
      rtb_WGS84_derivatives_ref_idx_1 = INS_DWork.Delay1_DSTATE_o[1];
    }
  
    /* End of Switch: '<S244>/Switch4' */
  
    /* Relay: '<S249>/Relay' incorporates:
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    if (rtb_Sum4_k >= INS_P.Relay_OnVal_e) {
      INS_DWork.Relay_Mode = true;
    } else {
      if (rtb_Sum4_k <= INS_P.Relay_OffVal_k) {
        INS_DWork.Relay_Mode = false;
      }
    }
  
    /* Relay: '<S249>/Relay1' incorporates:
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal ConversionInport1'
     */
    if (rtb_Sum2_j >= INS_P.Relay1_OnVal_n) {
      INS_DWork.Relay1_Mode = true;
    } else {
      if (rtb_Sum2_j <= INS_P.Relay1_OffVal_h) {
        INS_DWork.Relay1_Mode = false;
      }
    }
  
    /* Relay: '<S249>/Relay' */
    if (INS_DWork.Relay_Mode) {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOn_k;
    } else {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOff_k;
    }
  
    /* Relay: '<S249>/Relay1' */
    if (INS_DWork.Relay1_Mode) {
      u0 = INS_P.Relay1_YOn_p;
    } else {
      u0 = INS_P.Relay1_YOff_e;
    }
  
    /* RelationalOperator: '<S251>/Compare' incorporates:
     *  Constant: '<S251>/Constant'
     *  Logic: '<S249>/Logical Operator'
     *  MinMax: '<S249>/Max'
     */
    rtb_Delay_b = ((rtb_GPS_vaid && (fminf(rtb_Delay_m_idx_2, u0) != 0.0F)) >
                   (int32_T)INS_P.Constant_Value_cz);
  
    /* RelationalOperator: '<S252>/Compare' incorporates:
     *  Constant: '<S252>/Constant'
     *  SignalConversion: '<S269>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_FixPtRelationalOperator_l = ((int32_T)rtb_LogicalOperator_i > (int32_T)
      INS_P.Constant_Value_p2);
  
    /* Relay: '<S250>/Relay' incorporates:
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    if (rtb_Sum4_k >= INS_P.Relay_OnVal_e0) {
      INS_DWork.Relay_Mode_g = true;
    } else {
      if (rtb_Sum4_k <= INS_P.Relay_OffVal_p) {
        INS_DWork.Relay_Mode_g = false;
      }
    }
  
    /* Relay: '<S250>/Relay1' incorporates:
     *  SignalConversion: '<S267>/TmpSignal ConversionAtSignal ConversionInport1'
     */
    if (rtb_Sum2_j >= INS_P.Relay1_OnVal_c) {
      INS_DWork.Relay1_Mode_h = true;
    } else {
      if (rtb_Sum2_j <= INS_P.Relay1_OffVal_hg) {
        INS_DWork.Relay1_Mode_h = false;
      }
    }
  
    /* Relay: '<S250>/Relay' */
    if (INS_DWork.Relay_Mode_g) {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOn_l;
    } else {
      rtb_Delay_m_idx_2 = INS_P.Relay_YOff_m;
    }
  
    /* Relay: '<S250>/Relay1' */
    if (INS_DWork.Relay1_Mode_h) {
      u0 = INS_P.Relay1_YOn_e;
    } else {
      u0 = INS_P.Relay1_YOff_d;
    }
  
    /* Sum: '<S250>/Sum' incorporates:
     *  DataTypeConversion: '<S250>/Data Type Conversion1'
     *  Logic: '<S250>/Logical Operator'
     *  MinMax: '<S250>/Max'
     */
    rtb_Saturation1 = (rtb_GPS_vaid && (fminf(rtb_Delay_m_idx_2, u0) != 0.0F)) -
      rtb_FixPtRelationalOperator_l;
  
    /* Saturate: '<S250>/Saturation' */
    if (rtb_Saturation1 > INS_P.Saturation_UpperSat_c) {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation_UpperSat_c;
    } else if (rtb_Saturation1 < INS_P.Saturation_LowerSat_p) {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation_LowerSat_p;
    } else {
      rtb_DiscreteTimeIntegrator_df = (real32_T)rtb_Saturation1;
    }
  
    /* End of Saturate: '<S250>/Saturation' */
  
    /* RelationalOperator: '<S254>/Compare' incorporates:
     *  Constant: '<S254>/Constant'
     */
    rtb_Compare_f3 = (rtb_DiscreteTimeIntegrator_df > INS_P.Constant_Value_hv);
  
    /* RelationalOperator: '<S255>/Compare' incorporates:
     *  Constant: '<S255>/Constant'
     *  DataTypeConversion: '<S250>/Data Type Conversion1'
     */
    rtb_Compare_cio = ((real32_T)rtb_FixPtRelationalOperator_l >
                       INS_P.Constant_Value_h4);
  
    /* Sum: '<S250>/Subtract' incorporates:
     *  Constant: '<S253>/Constant'
     *  DataTypeConversion: '<S250>/Data Type Conversion1'
     *  DataTypeConversion: '<S250>/Data Type Conversion2'
     *  RelationalOperator: '<S253>/Compare'
     *  SignalConversion: '<S266>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_DiscreteTimeIntegrator_df = ((real32_T)((int32_T)
      rtb_FixPtRelationalOperator_if > (int32_T)INS_P.Constant_Value_hvm) -
      rtb_DiscreteTimeIntegrator_df) - (real32_T)rtb_FixPtRelationalOperator_l;
  
    /* Saturate: '<S250>/Saturation1' */
    if (rtb_DiscreteTimeIntegrator_df > INS_P.Saturation1_UpperSat_i) {
      rtb_DiscreteTimeIntegrator_df = INS_P.Saturation1_UpperSat_i;
    } else {
      if (rtb_DiscreteTimeIntegrator_df < INS_P.Saturation1_LowerSat_jb) {
        rtb_DiscreteTimeIntegrator_df = INS_P.Saturation1_LowerSat_jb;
      }
    }
  
    /* End of Saturate: '<S250>/Saturation1' */
  
    /* RelationalOperator: '<S256>/Compare' incorporates:
     *  Constant: '<S256>/Constant'
     */
    rtb_Compare_b = (rtb_DiscreteTimeIntegrator_df > INS_P.Constant_Value_iw);
  
    /* Update for UnitDelay: '<S238>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_j = rtb_Compare_le;
  
    /* Update for UnitDelay: '<S240>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_o = rtb_Compare_e;
  
    /* Update for Delay: '<S243>/Delay1' */
    INS_DWork.Delay1_DSTATE = rtb_Gain6;
  
    /* Update for UnitDelay: '<S245>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_dp = rtb_Compare_ag;
  
    /* Update for Delay: '<S244>/Delay' */
    INS_DWork.Delay_DSTATE[0] = rtb_WGS84_pos_ref_j_idx_0;
  
    /* Update for Delay: '<S244>/Delay1' */
    INS_DWork.Delay1_DSTATE_o[0] = rtb_WGS84_derivatives_ref_idx_0;
  
    /* Update for Delay: '<S244>/Delay' */
    INS_DWork.Delay_DSTATE[1] = rtb_Min;
  
    /* Update for Delay: '<S244>/Delay1' */
    INS_DWork.Delay1_DSTATE_o[1] = rtb_WGS84_derivatives_ref_idx_1;
  
    /* Update for UnitDelay: '<S247>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_m = rtb_Compare_os;
  
    /* End of Outputs for SubSystem: '<S57>/Process' */
  
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* Outputs for Enabled SubSystem: '<S222>/Reference_Height' incorporates:
     *  EnablePort: '<S225>/Enable'
     */
    if (rtb_Compare_f3) {
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* DataTypeConversion: '<S225>/Data Type Conversion' incorporates:
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion5Inport1'
       *  Sum: '<S225>/Sum'
       */
      INS_B.h_0 = (real32_T)(rtb_Scalefactor3 - rtb_Gain6);
  
      /* DataTypeConversion: '<S225>/Data Type Conversion1' incorporates:
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion8Inport1'
       */
      INS_B.vel_D_0 = rtb_vel_D;
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
    }
  
    /* End of Outputs for SubSystem: '<S222>/Reference_Height' */
  
    /* Outputs for Enabled SubSystem: '<S223>/Reference_Height' incorporates:
     *  EnablePort: '<S226>/Enable'
     */
    if (rtb_Compare_cio) {
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/Sonar_Process' */
      /* DataTypeConversion: '<S226>/Data Type Conversion' incorporates:
       *  Inport: '<Root>/Sonar'
       *  Product: '<S265>/Multiply'
       *  SignalConversion: '<S275>/TmpSignal ConversionAtSignal ConversionInport1'
       *  Trigonometry: '<S265>/Cos'
       *  Trigonometry: '<S265>/Cos1'
       */
      INS_B.h_0 = INS_U.Sonar.distance_m * arm_cos_f32
        (rtb_DiscreteTimeIntegrator_c__1) * arm_cos_f32
        (rtb_DiscreteTimeIntegrator_c__0);
  
      /* End of Outputs for SubSystem: '<S3>/Sonar_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
      /* End of Outputs for SubSystem: '<S57>/Process' */
  
      /* SignalConversion: '<S226>/OutportBuffer_InsertedFor_vel_D_mDs_at_inport_0' incorporates:
       *  Constant: '<S226>/Constant'
       */
      INS_B.vel_D_0 = INS_P.Constant_Value_k;
    }
  
    /* End of Outputs for SubSystem: '<S223>/Reference_Height' */
  
    /* Outputs for Enabled SubSystem: '<S221>/Reference_Height' incorporates:
     *  EnablePort: '<S224>/Enable'
     */
    if (rtb_Compare_b) {
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* DataTypeConversion: '<S224>/Data Type Conversion' incorporates:
       *  SignalConversion: '<S262>/TmpSignal ConversionAtSignal Copy1Inport1'
       */
      INS_B.h_0 = rtb_Product1_g0;
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
  
      /* SignalConversion: '<S224>/OutportBuffer_InsertedFor_vel_D_mDs_at_inport_0' incorporates:
       *  Constant: '<S224>/Constant'
       */
      INS_B.vel_D_0 = INS_P.Constant_Value_aa;
    }
  
    /* End of Outputs for SubSystem: '<S221>/Reference_Height' */
  
    /* Switch: '<S218>/Switch' incorporates:
     *  Constant: '<S220>/Constant'
     *  Delay: '<S218>/Delay'
     */
    if (rtb_LogicalOperator2_m) {
      rtb_ba_O_mDs2[0] = INS_B.h_0;
      rtb_ba_O_mDs2[1] = INS_B.vel_D_0;
      rtb_ba_O_mDs2[2] = INS_P.Constant_Value_eq;
    } else {
      rtb_ba_O_mDs2[0] = INS_DWork.Delay_DSTATE_b[0];
      rtb_ba_O_mDs2[1] = INS_DWork.Delay_DSTATE_b[1];
      rtb_ba_O_mDs2[2] = INS_DWork.Delay_DSTATE_b[2];
    }
  
    /* End of Switch: '<S218>/Switch' */
  
    /* Product: '<S219>/Multiply' incorporates:
     *  SignalConversion: '<S60>/TmpSignal ConversionAtSignal ConversionInport1'
     */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      /* Outputs for Atomic SubSystem: '<S56>/Integrate' */
      rtb_Product1_n[rtb_Saturation1] = rtb_VectorConcatenate_m[rtb_Saturation1 +
        6] * rtb_Switch3[2] + (rtb_VectorConcatenate_m[rtb_Saturation1 + 3] *
        rtb_Switch3[1] + rtb_VectorConcatenate_m[rtb_Saturation1] * rtb_Switch3[0]);
  
      /* End of Outputs for SubSystem: '<S56>/Integrate' */
    }
  
    /* Sum: '<S219>/Sum' incorporates:
     *  Constant: '<S219>/Constant'
     *  Constant: '<S219>/Constant1'
     *  Gain: '<S219>/Gain'
     *  Gain: '<S219>/Gain1'
     *  Product: '<S219>/Multiply'
     *  Sum: '<S219>/Add'
     */
    rtb_Divide1_m[0] = INS_P.Gain1_Gain_kw * rtb_ba_O_mDs2[1] * INS_P.Gain_Gain_mr
      + rtb_ba_O_mDs2[0];
    rtb_Divide1_m[1] = ((rtb_Product1_n[2] - rtb_ba_O_mDs2[2]) + INS_P.INS_CONST.g)
      * INS_P.Gain_Gain_mr + rtb_ba_O_mDs2[1];
    rtb_Divide1_m[2] = INS_P.Gain_Gain_mr * INS_P.Constant_Value_nn +
      rtb_ba_O_mDs2[2];
  
    /* Outputs for Enabled SubSystem: '<S198>/Baro_Correct' incorporates:
     *  EnablePort: '<S200>/Enable'
     */
    /* Switch: '<S198>/Switch' */
    if (rtb_Compare_b) {
      if (!INS_DWork.Baro_Correct_MODE) {
        /* InitializeConditions for UnitDelay: '<S201>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_lg = INS_P.DetectChange_vinit_m;
  
        /* InitializeConditions for Delay: '<S202>/Delay' */
        INS_DWork.Delay_DSTATE_dr[0] = INS_P.Delay_InitialCondition_j;
        INS_DWork.Delay_DSTATE_dr[1] = INS_P.Delay_InitialCondition_j;
        INS_DWork.Delay_DSTATE_dr[2] = INS_P.Delay_InitialCondition_j;
  
        /* InitializeConditions for Delay: '<S204>/x_delay' */
        INS_DWork.icLoad_a = 1U;
  
        /* InitializeConditions for UnitDelay: '<S208>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_f = INS_P.DetectIncrease_vinit_c;
  
        /* InitializeConditions for Delay: '<S206>/Delay' */
        INS_DWork.Delay_DSTATE_h = INS_P.Delay_InitialCondition_mk;
        INS_DWork.Baro_Correct_MODE = true;
      }
  
      /* Delay: '<S204>/x_delay' */
      if (rtb_LogicalOperator2_m && (INS_PrevZCSigState.x_delay_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_a = 1U;
      }
  
      INS_PrevZCSigState.x_delay_Reset_ZCE = rtb_LogicalOperator2_m;
      if (INS_DWork.icLoad_a != 0) {
        for (i = 0; i < 10; i++) {
          INS_DWork.x_delay_DSTATE[i] = rtb_Divide1_m[0];
        }
      }
  
      /* RelationalOperator: '<S208>/FixPt Relational Operator' incorporates:
       *  UnitDelay: '<S208>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (1 > (int32_T)INS_DWork.DelayInput1_DSTATE_f);
  
      /* Switch: '<S206>/Switch' incorporates:
       *  Delay: '<S206>/Delay'
       *  SignalConversion: '<S262>/TmpSignal ConversionAtSignal Copy1Inport1'
       *  Sum: '<S207>/Sum'
       */
      if (rtb_FixPtRelationalOperator_l) {
        /* Outputs for Atomic SubSystem: '<S57>/Process' */
        rtb_DiscreteTimeIntegrator_df = rtb_Divide1_m[0] - rtb_Product1_g0;
  
        /* End of Outputs for SubSystem: '<S57>/Process' */
      } else {
        rtb_DiscreteTimeIntegrator_df = INS_DWork.Delay_DSTATE_h;
      }
  
      /* End of Switch: '<S206>/Switch' */
  
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* Sum: '<S200>/Sum' incorporates:
       *  Delay: '<S204>/x_delay'
       *  SignalConversion: '<S262>/TmpSignal ConversionAtSignal Copy1Inport1'
       *  Sum: '<S203>/Sum1'
       */
      rtb_Product1_g0 = (rtb_Product1_g0 + rtb_DiscreteTimeIntegrator_df) -
        INS_DWork.x_delay_DSTATE[0];
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/Baro_Process' */
      /* Logic: '<S200>/Logical Operator' incorporates:
       *  Inport: '<Root>/Barometer'
       *  RelationalOperator: '<S201>/FixPt Relational Operator'
       *  SignalConversion: '<S270>/BusConversion_InsertedFor_Baro_Data_at_inport_0'
       *  UnitDelay: '<S201>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (rtb_FixPtRelationalOperator_l ||
        (INS_U.Barometer.timestamp != INS_DWork.DelayInput1_DSTATE_lg));
  
      /* End of Outputs for SubSystem: '<S3>/Baro_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Switch: '<S202>/Switch' incorporates:
       *  Delay: '<S202>/Delay'
       *  Gain: '<S205>/Gain'
       *  Gain: '<S205>/Gain1'
       *  Gain: '<S205>/Gain2'
       */
      if (rtb_FixPtRelationalOperator_l) {
        rtb_ba_O_mDs2[0] = INS_P.Gain2_Gain_m * rtb_Product1_g0;
        rtb_ba_O_mDs2[1] = INS_P.Gain1_Gain_e * rtb_Product1_g0;
        rtb_ba_O_mDs2[2] = INS_P.Gain_Gain_n * rtb_Product1_g0;
      } else {
        rtb_ba_O_mDs2[0] = INS_DWork.Delay_DSTATE_dr[0];
        rtb_ba_O_mDs2[1] = INS_DWork.Delay_DSTATE_dr[1];
        rtb_ba_O_mDs2[2] = INS_DWork.Delay_DSTATE_dr[2];
      }
  
      /* End of Switch: '<S202>/Switch' */
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/Baro_Process' */
      /* Update for UnitDelay: '<S201>/Delay Input1' incorporates:
       *  Inport: '<Root>/Barometer'
       *  SignalConversion: '<S270>/BusConversion_InsertedFor_Baro_Data_at_inport_0'
       */
      INS_DWork.DelayInput1_DSTATE_lg = INS_U.Barometer.timestamp;
  
      /* End of Outputs for SubSystem: '<S3>/Baro_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Sum: '<S200>/Sum1' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_B.Sum1_p[0] = INS_P.Gain_Gain_p * rtb_ba_O_mDs2[0] + rtb_Divide1_m[0];
  
      /* Update for Delay: '<S202>/Delay' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_DWork.Delay_DSTATE_dr[0] = rtb_ba_O_mDs2[0];
  
      /* Sum: '<S200>/Sum1' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_B.Sum1_p[1] = INS_P.Gain_Gain_p * rtb_ba_O_mDs2[1] + rtb_Divide1_m[1];
  
      /* Update for Delay: '<S202>/Delay' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_DWork.Delay_DSTATE_dr[1] = rtb_ba_O_mDs2[1];
  
      /* Sum: '<S200>/Sum1' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_B.Sum1_p[2] = INS_P.Gain_Gain_p * rtb_ba_O_mDs2[2] + rtb_Divide1_m[2];
  
      /* Update for Delay: '<S202>/Delay' incorporates:
       *  Gain: '<S202>/Gain'
       */
      INS_DWork.Delay_DSTATE_dr[2] = rtb_ba_O_mDs2[2];
  
      /* Update for Delay: '<S204>/x_delay' */
      INS_DWork.icLoad_a = 0U;
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 9; rtb_Saturation1++) {
        INS_DWork.x_delay_DSTATE[rtb_Saturation1] =
          INS_DWork.x_delay_DSTATE[rtb_Saturation1 + 1];
      }
  
      INS_DWork.x_delay_DSTATE[9] = rtb_Divide1_m[0];
  
      /* End of Update for Delay: '<S204>/x_delay' */
  
      /* Update for UnitDelay: '<S208>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_f = true;
  
      /* Update for Delay: '<S206>/Delay' */
      INS_DWork.Delay_DSTATE_h = rtb_DiscreteTimeIntegrator_df;
      rtb_Divide1_m[0] = INS_B.Sum1_p[0];
      rtb_Divide1_m[1] = INS_B.Sum1_p[1];
    } else {
      if (INS_DWork.Baro_Correct_MODE) {
        INS_DWork.Baro_Correct_MODE = false;
      }
    }
  
    /* End of Outputs for SubSystem: '<S198>/Baro_Correct' */
    u0 = rtb_Divide1_m[2];
    if (rtb_Compare_b) {
      u0 = INS_B.Sum1_p[2];
    }
  
    /* End of Switch: '<S198>/Switch' */
  
    /* Outputs for Enabled SubSystem: '<S199>/GPS_Correct' incorporates:
     *  EnablePort: '<S209>/Enable'
     */
    /* Switch: '<S199>/Switch' */
    if (rtb_Compare_f3) {
      if (!INS_DWork.GPS_Correct_MODE) {
        /* InitializeConditions for UnitDelay: '<S210>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_d = INS_P.DetectChange_vinit_h;
  
        /* InitializeConditions for Delay: '<S213>/h_delay' */
        INS_DWork.icLoad = 1U;
  
        /* InitializeConditions for Delay: '<S213>/vd_delay' */
        INS_DWork.icLoad_f = 1U;
  
        /* InitializeConditions for UnitDelay: '<S217>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_ib = INS_P.DetectIncrease_vinit_j;
  
        /* InitializeConditions for Memory: '<S215>/Memory' */
        INS_DWork.Memory_PreviousInput = INS_P.Memory_InitialCondition_p;
  
        /* InitializeConditions for Memory: '<S211>/Memory' */
        INS_DWork.Memory_PreviousInput_e[0] = INS_P.Memory_InitialCondition_c;
        INS_DWork.Memory_PreviousInput_e[1] = INS_P.Memory_InitialCondition_c;
        INS_DWork.Memory_PreviousInput_e[2] = INS_P.Memory_InitialCondition_c;
        INS_DWork.GPS_Correct_MODE = true;
      }
  
      /* Delay: '<S213>/h_delay' */
      if (rtb_LogicalOperator2_m && (INS_PrevZCSigState.h_delay_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad = 1U;
      }
  
      INS_PrevZCSigState.h_delay_Reset_ZCE = rtb_LogicalOperator2_m;
      if (INS_DWork.icLoad != 0) {
        for (i = 0; i < 75; i++) {
          INS_DWork.h_delay_DSTATE[i] = rtb_Divide1_m[0];
        }
      }
  
      /* Delay: '<S213>/vd_delay' */
      if (rtb_LogicalOperator2_m && (INS_PrevZCSigState.vd_delay_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_f = 1U;
      }
  
      INS_PrevZCSigState.vd_delay_Reset_ZCE = rtb_LogicalOperator2_m;
      if (INS_DWork.icLoad_f != 0) {
        for (i = 0; i < 50; i++) {
          INS_DWork.vd_delay_DSTATE[i] = rtb_Divide1_m[1];
        }
      }
  
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* DataTypeConversion: '<S212>/Data Type Conversion' incorporates:
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion5Inport1'
       *  Sum: '<S212>/Sum'
       */
      rtb_Product1_g0 = (real32_T)(rtb_Scalefactor3 - rtb_Gain6);
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
  
      /* RelationalOperator: '<S217>/FixPt Relational Operator' incorporates:
       *  UnitDelay: '<S217>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (1 > (int32_T)
        INS_DWork.DelayInput1_DSTATE_ib);
  
      /* Switch: '<S215>/Switch' incorporates:
       *  DataTypeConversion: '<S212>/Data Type Conversion'
       *  Memory: '<S215>/Memory'
       *  Sum: '<S216>/Sum'
       */
      if (rtb_FixPtRelationalOperator_l) {
        rtb_DiscreteTimeIntegrator_df = rtb_Divide1_m[0] - rtb_Product1_g0;
      } else {
        rtb_DiscreteTimeIntegrator_df = INS_DWork.Memory_PreviousInput;
      }
  
      /* End of Switch: '<S215>/Switch' */
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/Baro_Process' */
      /* Logic: '<S209>/Logical Operator' incorporates:
       *  Inport: '<Root>/Barometer'
       *  RelationalOperator: '<S210>/FixPt Relational Operator'
       *  SignalConversion: '<S270>/BusConversion_InsertedFor_Baro_Data_at_inport_0'
       *  UnitDelay: '<S210>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (rtb_FixPtRelationalOperator_l ||
        (INS_U.Barometer.timestamp != INS_DWork.DelayInput1_DSTATE_d));
  
      /* End of Outputs for SubSystem: '<S3>/Baro_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Switch: '<S211>/Switch' incorporates:
       *  DataTypeConversion: '<S212>/Data Type Conversion'
       *  Delay: '<S213>/h_delay'
       *  Gain: '<S214>/Gain'
       *  Gain: '<S214>/Gain1'
       *  Gain: '<S214>/Gain2'
       *  Memory: '<S211>/Memory'
       *  Sum: '<S209>/Sum'
       *  Sum: '<S212>/Sum1'
       */
      if (rtb_FixPtRelationalOperator_l) {
        /* Outputs for Atomic SubSystem: '<S57>/Process' */
        /* Sum: '<S209>/Sum' incorporates:
         *  Delay: '<S213>/vd_delay'
         *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion8Inport1'
         */
        rtb_Square_l_idx_1 = rtb_vel_D - INS_DWork.vd_delay_DSTATE[0U];
  
        /* End of Outputs for SubSystem: '<S57>/Process' */
        rtb_ba_O_mDs2[0] = ((rtb_Product1_g0 + rtb_DiscreteTimeIntegrator_df) -
                            INS_DWork.h_delay_DSTATE[0U]) * INS_P.Gain1_Gain_ml;
        rtb_ba_O_mDs2[1] = INS_P.Gain_Gain_c * rtb_Square_l_idx_1;
        rtb_ba_O_mDs2[2] = INS_P.Gain2_Gain_k * rtb_Square_l_idx_1;
      } else {
        rtb_ba_O_mDs2[0] = INS_DWork.Memory_PreviousInput_e[0];
        rtb_ba_O_mDs2[1] = INS_DWork.Memory_PreviousInput_e[1];
        rtb_ba_O_mDs2[2] = INS_DWork.Memory_PreviousInput_e[2];
      }
  
      /* End of Switch: '<S211>/Switch' */
  
      /* Sum: '<S209>/Sum1' incorporates:
       *  Gain: '<S211>/Gain3'
       */
      INS_B.Sum1[0] = INS_P.Gain3_Gain_kn * rtb_ba_O_mDs2[0] + rtb_Divide1_m[0];
      INS_B.Sum1[1] = INS_P.Gain3_Gain_kn * rtb_ba_O_mDs2[1] + rtb_Divide1_m[1];
      INS_B.Sum1[2] = INS_P.Gain3_Gain_kn * rtb_ba_O_mDs2[2] + u0;
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/Baro_Process' */
      /* Update for UnitDelay: '<S210>/Delay Input1' incorporates:
       *  Inport: '<Root>/Barometer'
       *  SignalConversion: '<S270>/BusConversion_InsertedFor_Baro_Data_at_inport_0'
       */
      INS_DWork.DelayInput1_DSTATE_d = INS_U.Barometer.timestamp;
  
      /* End of Outputs for SubSystem: '<S3>/Baro_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Update for Delay: '<S213>/h_delay' */
      INS_DWork.icLoad = 0U;
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 74; rtb_Saturation1++) {
        INS_DWork.h_delay_DSTATE[rtb_Saturation1] =
          INS_DWork.h_delay_DSTATE[rtb_Saturation1 + 1];
      }
  
      INS_DWork.h_delay_DSTATE[74] = rtb_Divide1_m[0];
  
      /* End of Update for Delay: '<S213>/h_delay' */
  
      /* Update for Delay: '<S213>/vd_delay' */
      INS_DWork.icLoad_f = 0U;
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 49; rtb_Saturation1++) {
        INS_DWork.vd_delay_DSTATE[rtb_Saturation1] =
          INS_DWork.vd_delay_DSTATE[rtb_Saturation1 + 1];
      }
  
      INS_DWork.vd_delay_DSTATE[49] = rtb_Divide1_m[1];
  
      /* End of Update for Delay: '<S213>/vd_delay' */
  
      /* Update for UnitDelay: '<S217>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_ib = true;
  
      /* Update for Memory: '<S215>/Memory' */
      INS_DWork.Memory_PreviousInput = rtb_DiscreteTimeIntegrator_df;
  
      /* Update for Memory: '<S211>/Memory' */
      INS_DWork.Memory_PreviousInput_e[0] = rtb_ba_O_mDs2[0];
      INS_DWork.Memory_PreviousInput_e[1] = rtb_ba_O_mDs2[1];
      INS_DWork.Memory_PreviousInput_e[2] = rtb_ba_O_mDs2[2];
      rtb_Divide1_m[0] = INS_B.Sum1[0];
      rtb_Divide1_m[1] = INS_B.Sum1[1];
      u0 = INS_B.Sum1[2];
    } else {
      if (INS_DWork.GPS_Correct_MODE) {
        INS_DWork.GPS_Correct_MODE = false;
      }
    }
  
    /* End of Switch: '<S199>/Switch' */
    /* End of Outputs for SubSystem: '<S199>/GPS_Correct' */
  
    /* Outputs for Atomic SubSystem: '<S57>/Process' */
    /* Switch: '<S172>/Switch' incorporates:
     *  SignalConversion: '<S269>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    if (rtb_LogicalOperator_i) {
      /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
       *  Constant: '<S166>/Constant'
       *  Outport: '<Root>/INS_Out'
       *  Sum: '<S172>/Sum'
       */
      INS_Y.INS_Out.h_AGL = rtb_Divide1_m[0] - INS_P.Constant_Value_aq;
    } else {
      /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
       *  Constant: '<S172>/Constant'
       *  Outport: '<Root>/INS_Out'
       */
      INS_Y.INS_Out.h_AGL = INS_P.Constant_Value_d;
    }
  
    /* End of Switch: '<S172>/Switch' */
    /* End of Outputs for SubSystem: '<S57>/Process' */
  
    /* Outputs for Enabled SubSystem: '<S192>/GPS_Vel_Init' incorporates:
     *  EnablePort: '<S194>/Enable'
     */
    /* Outputs for Enabled SubSystem: '<S191>/GPS_Pos_Init' incorporates:
     *  EnablePort: '<S193>/Enable'
     */
    if (rtb_GPS_vaid) {
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* DataTypeConversion: '<S193>/Data Type Conversion' incorporates:
       *  Product: '<S193>/Multiply'
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion3Inport1'
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion4Inport1'
       *  Sum: '<S193>/Sum'
       */
      INS_B.DataTypeConversion[0] = (real32_T)((rtb_Scalefactor1 -
        rtb_WGS84_pos_ref_j_idx_0) * rtb_WGS84_derivatives_ref_idx_0);
      INS_B.DataTypeConversion[1] = (real32_T)((rtb_Scalefactor2 - rtb_Min) *
        rtb_WGS84_derivatives_ref_idx_1);
  
      /* Reshape: '<S194>/Reshape' incorporates:
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion6Inport1'
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion7Inport1'
       */
      rtb_vel_NE_0_idx_0 = rtb_vel_N;
      rtb_vel_NE_0_idx_1 = rtb_vel_E;
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
    }
  
    /* End of Outputs for SubSystem: '<S191>/GPS_Pos_Init' */
    /* End of Outputs for SubSystem: '<S192>/GPS_Vel_Init' */
  
    /* Outputs for Enabled SubSystem: '<S192>/OptFlow_Vel_Init' incorporates:
     *  EnablePort: '<S195>/Enable'
     */
    /* Logic: '<S192>/Logical Operator' */
    if (!rtb_GPS_vaid) {
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* Reshape: '<S195>/Reshape' incorporates:
       *  SignalConversion: '<S264>/TmpSignal ConversionAtSignal Copy1Inport1'
       *  SignalConversion: '<S264>/TmpSignal ConversionAtSignal Copy2Inport1'
       */
      rtb_vel_NE_0_idx_0 = rtb_uT[0];
      rtb_vel_NE_0_idx_1 = rtb_uT[1];
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
    }
  
    /* End of Logic: '<S192>/Logical Operator' */
    /* End of Outputs for SubSystem: '<S192>/OptFlow_Vel_Init' */
  
    /* Switch: '<S188>/Switch' incorporates:
     *  Constant: '<S190>/Constant'
     *  Delay: '<S188>/Delay'
     */
    if (rtb_LogicalOperator2_o5) {
      rtb_Switch_er[0] = INS_B.DataTypeConversion[0];
      rtb_Switch_er[2] = rtb_vel_NE_0_idx_0;
      rtb_Switch_er[4] = INS_P.Constant_Value_b[0];
      rtb_Switch_er[1] = INS_B.DataTypeConversion[1];
      rtb_Switch_er[3] = rtb_vel_NE_0_idx_1;
      rtb_Switch_er[5] = INS_P.Constant_Value_b[1];
    } else {
      rtb_Switch_er[0] = INS_DWork.Delay_DSTATE_l[0];
      rtb_Switch_er[2] = INS_DWork.Delay_DSTATE_l[2];
      rtb_Switch_er[4] = INS_DWork.Delay_DSTATE_l[4];
      rtb_Switch_er[1] = INS_DWork.Delay_DSTATE_l[1];
      rtb_Switch_er[3] = INS_DWork.Delay_DSTATE_l[3];
      rtb_Switch_er[5] = INS_DWork.Delay_DSTATE_l[5];
    }
  
    /* End of Switch: '<S188>/Switch' */
  
    /* Product: '<S189>/Multiply' incorporates:
     *  SignalConversion: '<S60>/TmpSignal ConversionAtSignal ConversionInport1'
     */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      /* Outputs for Atomic SubSystem: '<S56>/Integrate' */
      rtb_Product1_n[rtb_Saturation1] = rtb_VectorConcatenate_m[rtb_Saturation1 +
        6] * rtb_Switch3[2] + (rtb_VectorConcatenate_m[rtb_Saturation1 + 3] *
        rtb_Switch3[1] + rtb_VectorConcatenate_m[rtb_Saturation1] * rtb_Switch3[0]);
  
      /* End of Outputs for SubSystem: '<S56>/Integrate' */
    }
  
    /* Gain: '<S189>/Gain' incorporates:
     *  Constant: '<S189>/Constant'
     *  Product: '<S189>/Multiply'
     *  Sum: '<S189>/Add'
     */
    tmp[0] = INS_P.Gain_Gain_ic * rtb_Switch_er[2];
    tmp[2] = (rtb_Product1_n[0] - rtb_Switch_er[4]) * INS_P.Gain_Gain_ic;
    tmp[4] = INS_P.Gain_Gain_ic * INS_P.Constant_Value_l[0];
    tmp[1] = INS_P.Gain_Gain_ic * rtb_Switch_er[3];
    tmp[3] = (rtb_Product1_n[1] - rtb_Switch_er[5]) * INS_P.Gain_Gain_ic;
    tmp[5] = INS_P.Gain_Gain_ic * INS_P.Constant_Value_l[1];
  
    /* Sum: '<S189>/Sum' */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 6; rtb_Saturation1++) {
      rtb_Sum_g0[rtb_Saturation1] = rtb_Switch_er[rtb_Saturation1] +
        tmp[rtb_Saturation1];
    }
  
    /* End of Sum: '<S189>/Sum' */
  
    /* Outputs for Enabled SubSystem: '<S176>/GPS_Correct' incorporates:
     *  EnablePort: '<S178>/Enable'
     */
    if (rtb_Delay_b) {
      if (!INS_DWork.GPS_Correct_MODE_o) {
        /* InitializeConditions for UnitDelay: '<S179>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_d0 = INS_P.DetectChange_vinit;
  
        /* InitializeConditions for Delay: '<S182>/x_delay' */
        INS_DWork.icLoad_o = 1U;
  
        /* InitializeConditions for Delay: '<S182>/x_delay1' */
        INS_DWork.icLoad_i = 1U;
  
        /* InitializeConditions for Delay: '<S182>/vn_delay' */
        INS_DWork.icLoad_m = 1U;
  
        /* InitializeConditions for Delay: '<S182>/vn_delay1' */
        INS_DWork.icLoad_j = 1U;
  
        /* InitializeConditions for UnitDelay: '<S187>/Delay Input1' */
        INS_DWork.DelayInput1_DSTATE_gn = INS_P.DetectIncrease_vinit_p;
  
        /* InitializeConditions for Memory: '<S185>/Memory' */
        INS_DWork.Memory_PreviousInput_d[0] = INS_P.Memory_InitialCondition[0];
        INS_DWork.Memory_PreviousInput_d[1] = INS_P.Memory_InitialCondition[1];
  
        /* InitializeConditions for Memory: '<S180>/Memory' */
        for (i = 0; i < 6; i++) {
          INS_DWork.Memory_PreviousInput_h[i] = INS_P.Memory_InitialCondition_o;
        }
  
        /* End of InitializeConditions for Memory: '<S180>/Memory' */
        INS_DWork.GPS_Correct_MODE_o = true;
      }
  
      /* Delay: '<S182>/x_delay' */
      if (rtb_LogicalOperator2_o5 && (INS_PrevZCSigState.x_delay_Reset_ZCE_b !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_o = 1U;
      }
  
      INS_PrevZCSigState.x_delay_Reset_ZCE_b = rtb_LogicalOperator2_o5;
      if (INS_DWork.icLoad_o != 0) {
        for (i = 0; i < 75; i++) {
          INS_DWork.x_delay_DSTATE_k[i] = rtb_Sum_g0[0];
        }
      }
  
      /* Delay: '<S182>/x_delay1' */
      if (rtb_LogicalOperator2_o5 && (INS_PrevZCSigState.x_delay1_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_i = 1U;
      }
  
      INS_PrevZCSigState.x_delay1_Reset_ZCE = rtb_LogicalOperator2_o5;
      if (INS_DWork.icLoad_i != 0) {
        for (i = 0; i < 75; i++) {
          INS_DWork.x_delay1_DSTATE[i] = rtb_Sum_g0[1];
        }
      }
  
      /* Delay: '<S182>/vn_delay' */
      if (rtb_LogicalOperator2_o5 && (INS_PrevZCSigState.vn_delay_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_m = 1U;
      }
  
      INS_PrevZCSigState.vn_delay_Reset_ZCE = rtb_LogicalOperator2_o5;
      if (INS_DWork.icLoad_m != 0) {
        for (i = 0; i < 50; i++) {
          INS_DWork.vn_delay_DSTATE[i] = rtb_Sum_g0[2];
        }
      }
  
      /* Delay: '<S182>/vn_delay1' */
      if (rtb_LogicalOperator2_o5 && (INS_PrevZCSigState.vn_delay1_Reset_ZCE !=
           POS_ZCSIG)) {
        INS_DWork.icLoad_j = 1U;
      }
  
      INS_PrevZCSigState.vn_delay1_Reset_ZCE = rtb_LogicalOperator2_o5;
      if (INS_DWork.icLoad_j != 0) {
        for (i = 0; i < 50; i++) {
          INS_DWork.vn_delay1_DSTATE[i] = rtb_Sum_g0[3];
        }
      }
  
      /* Outputs for Atomic SubSystem: '<S57>/Process' */
      /* DataTypeConversion: '<S181>/Data Type Conversion' incorporates:
       *  Product: '<S181>/Multiply'
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion3Inport1'
       *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion4Inport1'
       *  Sum: '<S181>/Sum'
       */
      rtb_vel_NE_0_idx_0 = (real32_T)((rtb_Scalefactor1 -
        rtb_WGS84_pos_ref_j_idx_0) * rtb_WGS84_derivatives_ref_idx_0);
      rtb_vel_NE_0_idx_1 = (real32_T)((rtb_Scalefactor2 - rtb_Min) *
        rtb_WGS84_derivatives_ref_idx_1);
  
      /* End of Outputs for SubSystem: '<S57>/Process' */
  
      /* RelationalOperator: '<S187>/FixPt Relational Operator' incorporates:
       *  UnitDelay: '<S187>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (1 > (int32_T)
        INS_DWork.DelayInput1_DSTATE_gn);
  
      /* Switch: '<S185>/Switch' incorporates:
       *  DataTypeConversion: '<S181>/Data Type Conversion'
       *  Memory: '<S185>/Memory'
       *  Sum: '<S186>/Sum'
       */
      if (rtb_FixPtRelationalOperator_l) {
        rtb_Square_l_idx_0 = rtb_Sum_g0[0] - rtb_vel_NE_0_idx_0;
        rtb_Square_l_idx_1 = rtb_Sum_g0[1] - rtb_vel_NE_0_idx_1;
      } else {
        rtb_Square_l_idx_0 = INS_DWork.Memory_PreviousInput_d[0];
        rtb_Square_l_idx_1 = INS_DWork.Memory_PreviousInput_d[1];
      }
  
      /* End of Switch: '<S185>/Switch' */
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/GPS_Process' */
      /* Logic: '<S178>/Logical Operator' incorporates:
       *  Inport: '<Root>/GPS_uBlox'
       *  RelationalOperator: '<S179>/FixPt Relational Operator'
       *  SignalConversion: '<S271>/BusConversion_InsertedFor_GPS_Data_at_inport_0'
       *  UnitDelay: '<S179>/Delay Input1'
       */
      rtb_FixPtRelationalOperator_l = (rtb_FixPtRelationalOperator_l ||
        (INS_U.GPS_uBlox.timestamp != INS_DWork.DelayInput1_DSTATE_d0));
  
      /* End of Outputs for SubSystem: '<S3>/GPS_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Switch: '<S180>/Switch' incorporates:
       *  DataTypeConversion: '<S181>/Data Type Conversion'
       *  Delay: '<S182>/x_delay'
       *  Delay: '<S182>/x_delay1'
       *  Gain: '<S183>/Gain'
       *  Gain: '<S183>/Gain2'
       *  Gain: '<S183>/Gain3'
       *  Memory: '<S180>/Memory'
       *  Sum: '<S178>/Sum'
       *  Sum: '<S184>/Sum1'
       */
      if (rtb_FixPtRelationalOperator_l) {
        /* Outputs for Atomic SubSystem: '<S57>/Process' */
        /* Sum: '<S178>/Sum' incorporates:
         *  Delay: '<S182>/vn_delay'
         *  Delay: '<S182>/vn_delay1'
         *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion6Inport1'
         *  SignalConversion: '<S263>/TmpSignal ConversionAtSignal Conversion7Inport1'
         */
        rtb_Delay_m_idx_2 = rtb_vel_N - INS_DWork.vn_delay_DSTATE[0U];
        rtb_DiscreteTimeIntegrator_df = rtb_vel_E - INS_DWork.vn_delay1_DSTATE[0U];
  
        /* End of Outputs for SubSystem: '<S57>/Process' */
        rtb_Switch_er[0] = ((rtb_vel_NE_0_idx_0 + rtb_Square_l_idx_0) -
                            INS_DWork.x_delay_DSTATE_k[0U]) * INS_P.Gain3_Gain_j;
        rtb_Switch_er[2] = INS_P.Gain_Gain_o0 * rtb_Delay_m_idx_2;
        rtb_Switch_er[4] = INS_P.Gain2_Gain_g * rtb_Delay_m_idx_2;
        rtb_Switch_er[1] = ((rtb_vel_NE_0_idx_1 + rtb_Square_l_idx_1) -
                            INS_DWork.x_delay1_DSTATE[0U]) * INS_P.Gain3_Gain_j;
        rtb_Switch_er[3] = INS_P.Gain_Gain_o0 * rtb_DiscreteTimeIntegrator_df;
        rtb_Switch_er[5] = INS_P.Gain2_Gain_g * rtb_DiscreteTimeIntegrator_df;
      } else {
        for (i = 0; i < 6; i++) {
          rtb_Switch_er[i] = INS_DWork.Memory_PreviousInput_h[i];
        }
      }
  
      /* End of Switch: '<S180>/Switch' */
  
      /* Sum: '<S178>/Sum1' incorporates:
       *  Gain: '<S180>/Gain3'
       */
      for (i = 0; i < 6; i++) {
        INS_B.Sum1_o[i] = INS_P.Gain3_Gain_o * rtb_Switch_er[i] + rtb_Sum_g0[i];
      }
  
      /* End of Sum: '<S178>/Sum1' */
  
      /* Outputs for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* Outputs for Atomic SubSystem: '<S3>/GPS_Process' */
      /* Update for UnitDelay: '<S179>/Delay Input1' incorporates:
       *  Inport: '<Root>/GPS_uBlox'
       *  SignalConversion: '<S271>/BusConversion_InsertedFor_GPS_Data_at_inport_0'
       */
      INS_DWork.DelayInput1_DSTATE_d0 = INS_U.GPS_uBlox.timestamp;
  
      /* End of Outputs for SubSystem: '<S3>/GPS_Process' */
      /* End of Outputs for SubSystem: '<Root>/Sensor_Process' */
  
      /* Update for Delay: '<S182>/x_delay' */
      INS_DWork.icLoad_o = 0U;
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 74; rtb_Saturation1++) {
        INS_DWork.x_delay_DSTATE_k[rtb_Saturation1] =
          INS_DWork.x_delay_DSTATE_k[rtb_Saturation1 + 1];
  
        /* Update for Delay: '<S182>/x_delay1' */
        INS_DWork.x_delay1_DSTATE[rtb_Saturation1] =
          INS_DWork.x_delay1_DSTATE[rtb_Saturation1 + 1];
      }
  
      INS_DWork.x_delay_DSTATE_k[74] = rtb_Sum_g0[0];
  
      /* End of Update for Delay: '<S182>/x_delay' */
  
      /* Update for Delay: '<S182>/x_delay1' */
      INS_DWork.icLoad_i = 0U;
      INS_DWork.x_delay1_DSTATE[74] = rtb_Sum_g0[1];
  
      /* Update for Delay: '<S182>/vn_delay' */
      INS_DWork.icLoad_m = 0U;
      for (rtb_Saturation1 = 0; rtb_Saturation1 < 49; rtb_Saturation1++) {
        INS_DWork.vn_delay_DSTATE[rtb_Saturation1] =
          INS_DWork.vn_delay_DSTATE[rtb_Saturation1 + 1];
  
        /* Update for Delay: '<S182>/vn_delay1' */
        INS_DWork.vn_delay1_DSTATE[rtb_Saturation1] =
          INS_DWork.vn_delay1_DSTATE[rtb_Saturation1 + 1];
      }
  
      INS_DWork.vn_delay_DSTATE[49] = rtb_Sum_g0[2];
  
      /* End of Update for Delay: '<S182>/vn_delay' */
  
      /* Update for Delay: '<S182>/vn_delay1' */
      INS_DWork.icLoad_j = 0U;
      INS_DWork.vn_delay1_DSTATE[49] = rtb_Sum_g0[3];
  
      /* Update for UnitDelay: '<S187>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_gn = true;
  
      /* Update for Memory: '<S185>/Memory' */
      INS_DWork.Memory_PreviousInput_d[0] = rtb_Square_l_idx_0;
      INS_DWork.Memory_PreviousInput_d[1] = rtb_Square_l_idx_1;
  
      /* Update for Memory: '<S180>/Memory' */
      for (i = 0; i < 6; i++) {
        INS_DWork.Memory_PreviousInput_h[i] = rtb_Switch_er[i];
      }
  
      /* End of Update for Memory: '<S180>/Memory' */
    } else {
      if (INS_DWork.GPS_Correct_MODE_o) {
        INS_DWork.GPS_Correct_MODE_o = false;
      }
    }
  
    /* End of Outputs for SubSystem: '<S176>/GPS_Correct' */
  
    /* Switch: '<S176>/Switch' */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 6; rtb_Saturation1++) {
      rtb_vel_NE_0_idx_0 = rtb_Sum_g0[rtb_Saturation1];
      if (rtb_Delay_b) {
        rtb_vel_NE_0_idx_0 = INS_B.Sum1_o[rtb_Saturation1];
      }
  
      rtb_Sum_g0[rtb_Saturation1] = rtb_vel_NE_0_idx_0;
    }
  
    /* End of Switch: '<S176>/Switch' */
  
    /* Abs: '<S173>/Abs' incorporates:
     *  Abs: '<S175>/Abs'
     */
    rtb_Scalefactor1 = fabs(rtb_WGS84_derivatives_ref_idx_0);
    rtb_Scalefactor2 = fabs(rtb_WGS84_derivatives_ref_idx_1);
  
    /* Switch: '<S173>/Switch' incorporates:
     *  Abs: '<S173>/Abs'
     *  Constant: '<S174>/Constant'
     *  MinMax: '<S173>/Min'
     *  RelationalOperator: '<S174>/Compare'
     *  Signum: '<S175>/Sign'
     */
    if (fmin(rtb_Scalefactor1, rtb_Scalefactor2) < INS_P.CompareToConstant_const)
    {
      /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
       *  Constant: '<S173>/Constant'
       *  Outport: '<Root>/INS_Out'
       */
      INS_Y.INS_Out.lat = INS_P.Constant_Value_i[0];
      INS_Y.INS_Out.lon = INS_P.Constant_Value_i[1];
      INS_Y.INS_Out.alt = INS_P.Constant_Value_i[2];
    } else {
      if (rtb_WGS84_derivatives_ref_idx_0 < 0.0) {
        /* Signum: '<S175>/Sign' */
        rtb_WGS84_derivatives_ref_idx_0 = -1.0;
      } else {
        if (rtb_WGS84_derivatives_ref_idx_0 > 0.0) {
          /* Signum: '<S175>/Sign' */
          rtb_WGS84_derivatives_ref_idx_0 = 1.0;
        }
      }
  
      /* Switch: '<S175>/Switch' incorporates:
       *  Constant: '<S175>/Constant'
       *  Signum: '<S175>/Sign'
       */
      if (rtb_WGS84_derivatives_ref_idx_0 == 0.0) {
        rtb_WGS84_derivatives_ref_idx_0 = INS_P.Constant_Value;
      }
  
      /* Saturate: '<S175>/Saturation' */
      if (rtb_Scalefactor1 > INS_P.Saturation_UpperSat) {
        rtb_Scalefactor1 = INS_P.Saturation_UpperSat;
      } else {
        if (rtb_Scalefactor1 < INS_P.Zero_Value_Exclusion_thr) {
          rtb_Scalefactor1 = INS_P.Zero_Value_Exclusion_thr;
        }
      }
  
      /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
       *  DataTypeConversion: '<S173>/Data Type Conversion'
       *  Outport: '<Root>/INS_Out'
       *  Product: '<S173>/Divide'
       *  Product: '<S175>/Product'
       *  Sum: '<S173>/Sum'
       */
      INS_Y.INS_Out.lat = rtb_Sum_g0[0] / (rtb_Scalefactor1 *
        rtb_WGS84_derivatives_ref_idx_0) + rtb_WGS84_pos_ref_j_idx_0;
  
      /* Signum: '<S175>/Sign' */
      if (rtb_WGS84_derivatives_ref_idx_1 < 0.0) {
        rtb_WGS84_derivatives_ref_idx_0 = -1.0;
      } else if (rtb_WGS84_derivatives_ref_idx_1 > 0.0) {
        rtb_WGS84_derivatives_ref_idx_0 = 1.0;
      } else {
        rtb_WGS84_derivatives_ref_idx_0 = rtb_WGS84_derivatives_ref_idx_1;
      }
  
      /* Switch: '<S175>/Switch' incorporates:
       *  Constant: '<S175>/Constant'
       */
      if (rtb_WGS84_derivatives_ref_idx_0 == 0.0) {
        rtb_WGS84_derivatives_ref_idx_0 = INS_P.Constant_Value;
      }
  
      /* Saturate: '<S175>/Saturation' */
      if (rtb_Scalefactor2 > INS_P.Saturation_UpperSat) {
        rtb_Scalefactor2 = INS_P.Saturation_UpperSat;
      } else {
        if (rtb_Scalefactor2 < INS_P.Zero_Value_Exclusion_thr) {
          rtb_Scalefactor2 = INS_P.Zero_Value_Exclusion_thr;
        }
      }
  
      /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
       *  DataTypeConversion: '<S173>/Data Type Conversion'
       *  DataTypeConversion: '<S173>/Data Type Conversion1'
       *  Outport: '<Root>/INS_Out'
       *  Product: '<S173>/Divide'
       *  Product: '<S175>/Product'
       *  Sum: '<S173>/Sum'
       */
      INS_Y.INS_Out.lon = rtb_Sum_g0[1] / (rtb_Scalefactor2 *
        rtb_WGS84_derivatives_ref_idx_0) + rtb_Min;
      INS_Y.INS_Out.alt = rtb_Divide1_m[0] + rtb_Gain6;
    }
  
    /* End of Switch: '<S173>/Switch' */
  
    /* Update for Delay: '<S218>/Delay' */
    INS_DWork.Delay_DSTATE_b[0] = rtb_Divide1_m[0];
    INS_DWork.Delay_DSTATE_b[1] = rtb_Divide1_m[1];
    INS_DWork.Delay_DSTATE_b[2] = u0;
  
    /* Update for Delay: '<S188>/Delay' */
    for (i = 0; i < 6; i++) {
      INS_DWork.Delay_DSTATE_l[i] = rtb_Sum_g0[i];
    }
  
    /* End of Update for Delay: '<S188>/Delay' */
    /* End of Outputs for SubSystem: '<S57>/CF' */
  
    /* Update for Delay: '<S56>/Delay' */
    for (i = 0; i < 9; i++) {
      INS_DWork.Delay_8_DSTATE_l[i] = rtb_VectorConcatenate_m[i];
      INS_DWork.Delay_9_DSTATE[i] = rtb_Transpose2[i];
    }
  
    INS_DWork.Delay_10_DSTATE[0] = rtb_VectorConcatenate_idx_0;
    INS_DWork.Delay_10_DSTATE[1] = rtb_VectorConcatenate_idx_1;
    INS_DWork.Delay_10_DSTATE[2] = rtb_DiscreteTimeIntegrator_c_id;
  
    /* End of Update for Delay: '<S56>/Delay' */
  
    /* Update for Delay: '<S57>/Delay' */
    INS_DWork.Delay_9_DSTATE_m = rtb_Sum_g0[0];
    INS_DWork.Delay_10_DSTATE_a = rtb_Sum_g0[1];
    INS_DWork.Delay_11_DSTATE = rtb_Divide1_m[0];
  
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* Outputs for Atomic SubSystem: '<Root>/Bus_Assignment' */
    /* Outputs for Atomic SubSystem: '<S1>/Counter' */
    /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
     *  Outport: '<Root>/INS_Out'
     *  UnitDelay: '<S4>/Output'
     */
    INS_Y.INS_Out.timestamp = INS_DWork.Output_DSTATE;
  
    /* Update for UnitDelay: '<S4>/Output' incorporates:
     *  Constant: '<S4>/Constant'
     *  Sum: '<S4>/Add'
     */
    INS_DWork.Output_DSTATE += INS_EXPORT.period;
  
    /* End of Outputs for SubSystem: '<S1>/Counter' */
  
    /* Outputs for Atomic SubSystem: '<S1>/Rotation_Output' */
    /* Sum: '<S5>/Sum' incorporates:
     *  Product: '<S5>/Multiply'
     *  SignalConversion: '<S5>/TmpSignal ConversionAtMultiplyInport2'
     *  SignalConversion: '<S60>/TmpSignal ConversionAtSignal ConversionInport1'
     */
    for (rtb_Saturation1 = 0; rtb_Saturation1 < 3; rtb_Saturation1++) {
      /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
      /* Outputs for Atomic SubSystem: '<S56>/Integrate' */
      rtb_ba_O_mDs2[rtb_Saturation1] = rtb_Switch3[rtb_Saturation1] -
        ((rtb_Transpose2[rtb_Saturation1 + 3] * rtb_Sum_g0[5] +
          rtb_Transpose2[rtb_Saturation1] * rtb_Sum_g0[4]) +
         rtb_Transpose2[rtb_Saturation1 + 6] * u0);
  
      /* End of Outputs for SubSystem: '<S56>/Integrate' */
      /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
    }
  
    /* End of Sum: '<S5>/Sum' */
    /* End of Outputs for SubSystem: '<S1>/Rotation_Output' */
  
    /* Outputs for Atomic SubSystem: '<S1>/Status_Output' */
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* RelationalOperator: '<S25>/Compare' incorporates:
     *  Constant: '<S25>/Constant'
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_FixPtRelationalOperator_l = ((int32_T)rtb_Delay_b > (int32_T)
      INS_P.CompareToConstant1_const);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* DiscreteIntegrator: '<S13>/Discrete-Time Integrator' */
    if (rtb_FixPtRelationalOperator_l ||
        (INS_DWork.DiscreteTimeIntegrator_PrevRe_n != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_b = INS_P.DiscreteTimeIntegrator_IC;
    }
  
    /* RelationalOperator: '<S40>/Compare' incorporates:
     *  Constant: '<S40>/Constant'
     */
    rtb_GPS_vaid = ((int32_T)rtb_Compare_fz > (int32_T)
                    INS_P.CompareToConstant1_const_j);
  
    /* DiscreteIntegrator: '<S16>/Discrete-Time Integrator' */
    if (rtb_GPS_vaid || (INS_DWork.DiscreteTimeIntegrator_PrevRe_m != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_h =
        INS_P.DiscreteTimeIntegrator_IC_i;
    }
  
    /* Relay: '<S10>/Relay1' */
    if (rtb_DiscreteTimeIntegrator_g >= INS_P.Relay1_OnVal) {
      INS_DWork.Relay1_Mode_j = true;
    } else {
      if (rtb_DiscreteTimeIntegrator_g <= INS_P.Relay1_OffVal) {
        INS_DWork.Relay1_Mode_j = false;
      }
    }
  
    if (INS_DWork.Relay1_Mode_j) {
      rtb_heading_quality = INS_P.Relay1_YOn_i;
    } else {
      rtb_heading_quality = INS_P.Relay1_YOff_k;
    }
  
    /* End of Relay: '<S10>/Relay1' */
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* MinMax: '<S10>/Max' incorporates:
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_heading_quality = (rtb_Delay_b || rtb_heading_quality);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S50>/Compare' incorporates:
     *  Constant: '<S50>/Constant'
     */
    rtb_Compare_le = ((int32_T)rtb_heading_quality > (int32_T)
                      INS_P.CompareToConstant1_const_g);
  
    /* DiscreteIntegrator: '<S18>/Discrete-Time Integrator' */
    if (rtb_Compare_le || (INS_DWork.DiscreteTimeIntegrator_PrevRe_d != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_b3 =
        INS_P.DiscreteTimeIntegrator_IC_o;
    }
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/Process' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* MinMax: '<S10>/Max3' incorporates:
     *  SignalConversion: '<S168>/Signal Copy'
     *  SignalConversion: '<S268>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_LogicalOperator2_o5 = (rtb_LogicalOperator3 || rtb_Delay_b);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<S57>/Process' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S20>/Compare' incorporates:
     *  Constant: '<S20>/Constant'
     */
    rtb_LogicalOperator2_m = ((int32_T)rtb_LogicalOperator2_o5 > (int32_T)
      INS_P.CompareToConstant1_const_e);
  
    /* DiscreteIntegrator: '<S12>/Discrete-Time Integrator' */
    if (rtb_LogicalOperator2_m || (INS_DWork.DiscreteTimeIntegrator_PrevRe_h != 0))
    {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ca =
        INS_P.DiscreteTimeIntegrator_IC_l;
    }
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* Outputs for Atomic SubSystem: '<S57>/Process' */
    /* MinMax: '<S10>/Max1' incorporates:
     *  SignalConversion: '<S168>/Signal Copy'
     *  SignalConversion: '<S268>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_Compare_ag = (rtb_Delay_b || rtb_LogicalOperator3);
  
    /* End of Outputs for SubSystem: '<S57>/Process' */
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S30>/Compare' incorporates:
     *  Constant: '<S30>/Constant'
     */
    rtb_Compare_e = ((int32_T)rtb_Compare_ag > (int32_T)
                     INS_P.CompareToConstant1_const_ei);
  
    /* DiscreteIntegrator: '<S14>/Discrete-Time Integrator' */
    if (rtb_Compare_e || (INS_DWork.DiscreteTimeIntegrator_PrevRe_a != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_e =
        INS_P.DiscreteTimeIntegrator_IC_j;
    }
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* MinMax: '<S10>/Max2' incorporates:
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_Compare_os = (rtb_Compare_f3 || rtb_Compare_b || rtb_Compare_cio);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S35>/Compare' incorporates:
     *  Constant: '<S35>/Constant'
     */
    rtb_Compare_f3 = ((int32_T)rtb_Compare_os > (int32_T)
                      INS_P.CompareToConstant1_const_ec);
  
    /* DiscreteIntegrator: '<S15>/Discrete-Time Integrator' */
    if (rtb_Compare_f3 || (INS_DWork.DiscreteTimeIntegrator_PrevRe_o != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kg =
        INS_P.DiscreteTimeIntegrator_IC_ig;
    }
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* RelationalOperator: '<S45>/Compare' incorporates:
     *  Constant: '<S45>/Constant'
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_Compare_b = ((int32_T)rtb_Compare_cio > (int32_T)
                     INS_P.CompareToConstant1_const_em);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* DiscreteIntegrator: '<S17>/Discrete-Time Integrator' */
    if (rtb_Compare_b || (INS_DWork.DiscreteTimeIntegrator_PrevR_ar != 0)) {
      INS_DWork.DiscreteTimeIntegrator_DSTATE_a =
        INS_P.DiscreteTimeIntegrator_IC_e;
    }
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S56>/Integrate' */
    /* Gain: '<S8>/Gain10' incorporates:
     *  SignalConversion: '<S60>/TmpSignal ConversionAtSignal Conversion1Inport1'
     */
    rtb_Delay_m_idx_2 = fmodf(floorf(INS_P.Gain10_Gain * rtb_Relay2),
      4.2949673E+9F);
  
    /* End of Outputs for SubSystem: '<S56>/Integrate' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* Sum: '<S8>/Sum2' incorporates:
     *  Constant: '<S168>/Constant'
     *  Constant: '<S19>/Constant'
     *  Constant: '<S24>/Constant'
     *  Constant: '<S29>/Constant'
     *  Constant: '<S34>/Constant'
     *  Constant: '<S39>/Constant'
     *  Constant: '<S44>/Constant'
     *  Constant: '<S49>/Constant'
     *  Delay: '<S12>/Delay'
     *  Delay: '<S13>/Delay'
     *  Delay: '<S14>/Delay'
     *  Delay: '<S15>/Delay'
     *  Delay: '<S16>/Delay'
     *  Delay: '<S17>/Delay'
     *  Delay: '<S18>/Delay'
     *  DiscreteIntegrator: '<S12>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S13>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S14>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S15>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S16>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
     *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
     *  Gain: '<S8>/Gain1'
     *  Gain: '<S8>/Gain10'
     *  Gain: '<S8>/Gain11'
     *  Gain: '<S8>/Gain12'
     *  Gain: '<S8>/Gain13'
     *  Gain: '<S8>/Gain2'
     *  Gain: '<S8>/Gain4'
     *  Gain: '<S8>/Gain5'
     *  Gain: '<S8>/Gain9'
     *  Logic: '<S12>/Logical Operator'
     *  Logic: '<S13>/Logical Operator'
     *  Logic: '<S14>/Logical Operator'
     *  Logic: '<S15>/Logical Operator'
     *  Logic: '<S16>/Logical Operator'
     *  Logic: '<S17>/Logical Operator'
     *  Logic: '<S18>/Logical Operator'
     *  Logic: '<S9>/Logical Operator'
     *  Product: '<S12>/Multiply'
     *  Product: '<S13>/Multiply'
     *  Product: '<S14>/Multiply'
     *  Product: '<S15>/Multiply'
     *  Product: '<S16>/Multiply'
     *  Product: '<S17>/Multiply'
     *  Product: '<S18>/Multiply'
     *  RelationalOperator: '<S19>/Compare'
     *  RelationalOperator: '<S24>/Compare'
     *  RelationalOperator: '<S29>/Compare'
     *  RelationalOperator: '<S34>/Compare'
     *  RelationalOperator: '<S39>/Compare'
     *  RelationalOperator: '<S44>/Compare'
     *  RelationalOperator: '<S49>/Compare'
     */
    INS_Y.INS_Out.flag = (((((((((INS_P.Constant_Value_jz != 0) && (rtb_Compare_cl
      != 0) ? (uint32_T)INS_P.Gain9_Gain >> 7 : 0U) + (rtb_Delay_m_idx_2 < 0.0F ?
      (uint32_T)-(int32_T)(uint32_T)-rtb_Delay_m_idx_2 : (uint32_T)
      rtb_Delay_m_idx_2)) + (((uint32_T)(INS_DWork.DiscreteTimeIntegrator_DSTATE_h
      < INS_P.valid_check5_time_out ? (int32_T)INS_DWork.Delay_DSTATE_c : 0) *
      INS_P.Gain11_Gain) >> 13)) + (((uint32_T)
      (INS_DWork.DiscreteTimeIntegrator_DSTAT_b3 < INS_P.valid_check8_time_out ?
       (int32_T)INS_DWork.Delay_DSTATE_l0 : 0) * INS_P.Gain12_Gain) >> 12)) +
      (((uint32_T)(INS_DWork.DiscreteTimeIntegrator_DSTAT_ca <
                   INS_P.valid_check1_time_out ? (int32_T)
                   INS_DWork.Delay_DSTATE_cn : 0) * INS_P.Gain13_Gain) >> 11)) +
      (((uint32_T)(INS_DWork.DiscreteTimeIntegrator_DSTATE_b <
                   INS_P.valid_check2_time_out ? (int32_T)
                   INS_DWork.Delay_DSTATE_bn : 0) * INS_P.Gain1_Gain_kq) >> 10)) +
      (((uint32_T)(INS_DWork.DiscreteTimeIntegrator_DSTATE_e <
                   INS_P.valid_check3_time_out ? (int32_T)INS_DWork.Delay_DSTATE_p
                   : 0) * INS_P.Gain2_Gain_o5) >> 9)) + (((uint32_T)
      (INS_DWork.DiscreteTimeIntegrator_DSTAT_kg < INS_P.valid_check4_time_out ?
       (int32_T)INS_DWork.Delay_DSTATE_db : 0) * INS_P.Gain4_Gain_a) >> 8)) +
      (((uint32_T)(INS_DWork.DiscreteTimeIntegrator_DSTATE_a <
                   INS_P.valid_check6_time_out ? (int32_T)
                   INS_DWork.Delay_DSTATE_ly : 0) * INS_P.Gain5_Gain_e) >> 7);
  
    /* RelationalOperator: '<S23>/Compare' incorporates:
     *  Constant: '<S21>/Constant'
     *  Constant: '<S23>/Constant'
     *  RelationalOperator: '<S21>/Compare'
     */
    rtb_LogicalOperator2_o5 = (((int32_T)rtb_LogicalOperator2_o5 > (int32_T)
      INS_P.CompareToConstant2_const_p) > (int32_T)INS_P.Constant_Value_mt);
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* RelationalOperator: '<S28>/Compare' incorporates:
     *  Constant: '<S26>/Constant'
     *  Constant: '<S28>/Constant'
     *  RelationalOperator: '<S26>/Compare'
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_Delay_b = (((int32_T)rtb_Delay_b > (int32_T)
                    INS_P.CompareToConstant2_const_c) > (int32_T)
                   INS_P.Constant_Value_m2);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S33>/Compare' incorporates:
     *  Constant: '<S31>/Constant'
     *  Constant: '<S33>/Constant'
     *  RelationalOperator: '<S31>/Compare'
     */
    rtb_Compare_ag = (((int32_T)rtb_Compare_ag > (int32_T)
                       INS_P.CompareToConstant2_const_n) > (int32_T)
                      INS_P.Constant_Value_o2);
  
    /* RelationalOperator: '<S38>/Compare' incorporates:
     *  Constant: '<S36>/Constant'
     *  Constant: '<S38>/Constant'
     *  RelationalOperator: '<S36>/Compare'
     */
    rtb_Compare_os = (((int32_T)rtb_Compare_os > (int32_T)
                       INS_P.CompareToConstant2_const_b) > (int32_T)
                      INS_P.Constant_Value_hk);
  
    /* RelationalOperator: '<S43>/Compare' incorporates:
     *  Constant: '<S41>/Constant'
     *  Constant: '<S43>/Constant'
     *  RelationalOperator: '<S41>/Compare'
     */
    rtb_Compare_fz = (((int32_T)rtb_Compare_fz > (int32_T)
                       INS_P.CompareToConstant2_const_i) > (int32_T)
                      INS_P.Constant_Value_l1);
  
    /* Outputs for Atomic SubSystem: '<Root>/Data_Fusion' */
    /* Outputs for Atomic SubSystem: '<S57>/CF' */
    /* RelationalOperator: '<S48>/Compare' incorporates:
     *  Constant: '<S46>/Constant'
     *  Constant: '<S48>/Constant'
     *  RelationalOperator: '<S46>/Compare'
     *  SignalConversion: '<S168>/Signal Copy'
     */
    rtb_Compare_cio = (((int32_T)rtb_Compare_cio > (int32_T)
                        INS_P.CompareToConstant2_const_l) > (int32_T)
                       INS_P.Constant_Value_m2n);
  
    /* End of Outputs for SubSystem: '<S57>/CF' */
    /* End of Outputs for SubSystem: '<Root>/Data_Fusion' */
  
    /* RelationalOperator: '<S53>/Compare' incorporates:
     *  Constant: '<S51>/Constant'
     *  Constant: '<S53>/Constant'
     *  RelationalOperator: '<S51>/Compare'
     */
    rtb_heading_quality = (((int32_T)rtb_heading_quality > (int32_T)
      INS_P.CompareToConstant2_const_i1) > (int32_T)INS_P.Constant_Value_ao);
  
    /* Sum: '<S11>/Sum' incorporates:
     *  Constant: '<S54>/Constant'
     *  Constant: '<S55>/Constant'
     *  DataTypeConversion: '<S11>/Data Type Conversion'
     *  Gain: '<S11>/Gain2'
     *  Gain: '<S11>/Gain3'
     *  Gain: '<S11>/Gain4'
     *  Gain: '<S11>/Gain5'
     *  Gain: '<S11>/Gain7'
     *  MinMax: '<S11>/Max'
     *  RelationalOperator: '<S54>/Compare'
     *  RelationalOperator: '<S55>/Compare'
     *  Sum: '<S11>/Sum2'
     */
    INS_Y.INS_Out.status = (((((rtb_DiscreteTimeIntegrator_g >
      INS_P.Constant_Value_h ? (uint32_T)INS_P.Gain2_Gain_ha >> 5 : 0U) +
      (rtb_FixPtRelationalOperator_if ? (uint32_T)INS_P.Gain3_Gain_f >> 4 : 0U)) +
      (fmaxf(rtb_Sum2_j, rtb_Sum4_k) > INS_P.Constant_Value_j ? (uint32_T)
       INS_P.Gain4_Gain_p >> 3 : 0U)) + (rtb_LogicalOperator_i ? (uint32_T)
      INS_P.Gain5_Gain_o >> 2 : 0U)) + rtb_Delay_h) + (rtb_LogicalOperator3 ?
      (uint32_T)INS_P.Gain7_Gain >> 1 : 0U);
  
    /* Update for DiscreteIntegrator: '<S13>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S13>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_b +=
      INS_P.DiscreteTimeIntegrator_gainval * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_n = (int8_T)
      rtb_FixPtRelationalOperator_l;
  
    /* Update for Delay: '<S13>/Delay' incorporates:
     *  Logic: '<S13>/Logical Operator1'
     *  RelationalOperator: '<S27>/FixPt Relational Operator'
     *  UnitDelay: '<S27>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_bn = (((int32_T)rtb_Delay_b > (int32_T)
      INS_DWork.DelayInput1_DSTATE_mu) || INS_DWork.Delay_DSTATE_bn);
  
    /* Update for DiscreteIntegrator: '<S16>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S16>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_h +=
      INS_P.DiscreteTimeIntegrator_gainva_i * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_m = (int8_T)rtb_GPS_vaid;
  
    /* Update for Delay: '<S16>/Delay' incorporates:
     *  Logic: '<S16>/Logical Operator1'
     *  RelationalOperator: '<S42>/FixPt Relational Operator'
     *  UnitDelay: '<S42>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_c = (((int32_T)rtb_Compare_fz > (int32_T)
      INS_DWork.DelayInput1_DSTATE_ng) || INS_DWork.Delay_DSTATE_c);
  
    /* Update for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S18>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTAT_b3 +=
      INS_P.DiscreteTimeIntegrator_gainva_m * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_d = (int8_T)rtb_Compare_le;
  
    /* Update for Delay: '<S18>/Delay' incorporates:
     *  Logic: '<S18>/Logical Operator1'
     *  RelationalOperator: '<S52>/FixPt Relational Operator'
     *  UnitDelay: '<S52>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_l0 = (((int32_T)rtb_heading_quality > (int32_T)
      INS_DWork.DelayInput1_DSTATE_fq) || INS_DWork.Delay_DSTATE_l0);
  
    /* Update for DiscreteIntegrator: '<S12>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S12>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTAT_ca +=
      INS_P.DiscreteTimeIntegrator_gainva_k * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_h = (int8_T)rtb_LogicalOperator2_m;
  
    /* Update for Delay: '<S12>/Delay' incorporates:
     *  Logic: '<S12>/Logical Operator1'
     *  RelationalOperator: '<S22>/FixPt Relational Operator'
     *  UnitDelay: '<S22>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_cn = (((int32_T)rtb_LogicalOperator2_o5 > (int32_T)
      INS_DWork.DelayInput1_DSTATE_dx) || INS_DWork.Delay_DSTATE_cn);
  
    /* Update for DiscreteIntegrator: '<S14>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S14>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_e +=
      INS_P.DiscreteTimeIntegrator_gainva_p * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_a = (int8_T)rtb_Compare_e;
  
    /* Update for Delay: '<S14>/Delay' incorporates:
     *  Logic: '<S14>/Logical Operator1'
     *  RelationalOperator: '<S32>/FixPt Relational Operator'
     *  UnitDelay: '<S32>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_p = (((int32_T)rtb_Compare_ag > (int32_T)
      INS_DWork.DelayInput1_DSTATE_dq) || INS_DWork.Delay_DSTATE_p);
  
    /* Update for DiscreteIntegrator: '<S15>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S15>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTAT_kg +=
      INS_P.DiscreteTimeIntegrator_gainv_me * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevRe_o = (int8_T)rtb_Compare_f3;
  
    /* Update for Delay: '<S15>/Delay' incorporates:
     *  Logic: '<S15>/Logical Operator1'
     *  RelationalOperator: '<S37>/FixPt Relational Operator'
     *  UnitDelay: '<S37>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_db = (((int32_T)rtb_Compare_os > (int32_T)
      INS_DWork.DelayInput1_DSTATE_f4) || INS_DWork.Delay_DSTATE_db);
  
    /* Update for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' incorporates:
     *  Constant: '<S17>/Constant'
     */
    INS_DWork.DiscreteTimeIntegrator_DSTATE_a +=
      INS_P.DiscreteTimeIntegrator_gainv_my * (real32_T)INS_EXPORT.period;
    INS_DWork.DiscreteTimeIntegrator_PrevR_ar = (int8_T)rtb_Compare_b;
  
    /* Update for Delay: '<S17>/Delay' incorporates:
     *  Logic: '<S17>/Logical Operator1'
     *  RelationalOperator: '<S47>/FixPt Relational Operator'
     *  UnitDelay: '<S47>/Delay Input1'
     */
    INS_DWork.Delay_DSTATE_ly = (((int32_T)rtb_Compare_cio > (int32_T)
      INS_DWork.DelayInput1_DSTATE_dql) || INS_DWork.Delay_DSTATE_ly);
  
    /* Update for UnitDelay: '<S22>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_dx = rtb_LogicalOperator2_o5;
  
    /* Update for UnitDelay: '<S27>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_mu = rtb_Delay_b;
  
    /* Update for UnitDelay: '<S32>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_dq = rtb_Compare_ag;
  
    /* Update for UnitDelay: '<S37>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_f4 = rtb_Compare_os;
  
    /* Update for UnitDelay: '<S42>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_ng = rtb_Compare_fz;
  
    /* Update for UnitDelay: '<S47>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_dql = rtb_Compare_cio;
  
    /* Update for UnitDelay: '<S52>/Delay Input1' */
    INS_DWork.DelayInput1_DSTATE_fq = rtb_heading_quality;
  
    /* End of Outputs for SubSystem: '<S1>/Status_Output' */
  
    /* Outputs for Atomic SubSystem: '<S1>/Rotation_Output' */
    /* BusCreator: '<Root>/BusConversion_InsertedFor_INS_Out_at_inport_0' incorporates:
     *  Constant: '<S7>/Constant'
     *  Outport: '<Root>/INS_Out'
     *  SignalConversion: '<S5>/Signal Copy'
     *  SignalConversion: '<S7>/Signal Copy'
     */
    INS_Y.INS_Out.phi = rtb_DiscreteTimeIntegrator_c__1;
    INS_Y.INS_Out.theta = rtb_DiscreteTimeIntegrator_c__0;
    INS_Y.INS_Out.psi = rtb_TrigonometricFunction1;
    INS_Y.INS_Out.quat[0] = rtb_Add_m;
    INS_Y.INS_Out.quat[1] = rtb_Inclination;
    INS_Y.INS_Out.quat[2] = rtb_Delay_m_idx_1;
    INS_Y.INS_Out.quat[3] = rtb_Product3_eb;
    INS_Y.INS_Out.p = rtb_VectorConcatenate_idx_0;
    INS_Y.INS_Out.q = rtb_VectorConcatenate_idx_1;
  
    /* End of Outputs for SubSystem: '<S1>/Rotation_Output' */
    /* End of Outputs for SubSystem: '<Root>/Bus_Assignment' */
    INS_Y.INS_Out.r = rtb_DiscreteTimeIntegrator_c_id;
    INS_Y.INS_Out.ax = rtb_ba_O_mDs2[0];
    INS_Y.INS_Out.ay = rtb_ba_O_mDs2[1];
    INS_Y.INS_Out.az = rtb_ba_O_mDs2[2];
  
    /* Outputs for Atomic SubSystem: '<Root>/Bus_Assignment' */
    /* Outputs for Atomic SubSystem: '<S1>/Translation_Output' */
    INS_Y.INS_Out.vn = rtb_Sum_g0[2];
    INS_Y.INS_Out.ve = rtb_Sum_g0[3];
    INS_Y.INS_Out.vd = rtb_Divide1_m[1];
  
    /* End of Outputs for SubSystem: '<S1>/Translation_Output' */
    /* End of Outputs for SubSystem: '<Root>/Bus_Assignment' */
    INS_Y.INS_Out.reserved = INS_P.Constant_Value_n;
  
    /* Outputs for Atomic SubSystem: '<Root>/Bus_Assignment' */
    /* Outputs for Atomic SubSystem: '<S1>/Translation_Output' */
    INS_Y.INS_Out.x_R = rtb_Sum_g0[0];
    INS_Y.INS_Out.y_R = rtb_Sum_g0[1];
    INS_Y.INS_Out.h_R = rtb_Divide1_m[0];
  
    /* End of Outputs for SubSystem: '<S1>/Translation_Output' */
    /* End of Outputs for SubSystem: '<Root>/Bus_Assignment' */
  
    /* Update for Delay: '<Root>/Delay1' */
    INS_DWork.Delay1_4_DSTATE = rtb_Sum_g0[2];
    INS_DWork.Delay1_5_DSTATE = rtb_Sum_g0[3];
  
    /* Update for Delay: '<Root>/Delay' */
    for (i = 0; i < 9; i++) {
      INS_DWork.Delay_8_DSTATE[i] = rtb_VectorConcatenate_m[i];
    }
  }
  
  /* Model initialize function */
  void INS_init(void)
  {
    /* Registration code */
  
    /* initialize error status */
    rtmSetErrorStatus(INS_M, (NULL));
  
    /* block I/O */
    (void) memset(((void *) &INS_B), 0,
                  sizeof(BlockIO_INS_T));
  
    /* states (dwork) */
    (void) memset((void *)&INS_DWork, 0,
                  sizeof(D_Work_INS_T));
  
    /* external inputs */
    (void)memset(&INS_U, 0, sizeof(ExternalInputs_INS_T));
  
    /* external outputs */
    INS_Y.INS_Out = INS_rtZINS_Out_Bus;
  
    {
      int32_T i;
      INS_PrevZCSigState.x_delay_Reset_ZCE_b = POS_ZCSIG;
      INS_PrevZCSigState.x_delay1_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.vn_delay_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.vn_delay1_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.x_delay_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.h_delay_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.vd_delay_Reset_ZCE = POS_ZCSIG;
      INS_PrevZCSigState.WGS84_Model_Trig_ZCE = POS_ZCSIG;
  
      /* InitializeConditions for Delay: '<Root>/Delay1' */
      INS_DWork.Delay1_4_DSTATE = INS_P.Delay1_4_InitialCondition;
      INS_DWork.Delay1_5_DSTATE = INS_P.Delay1_5_InitialCondition;
  
      /* SystemInitialize for Atomic SubSystem: '<Root>/Sensor_Process' */
      /* SystemInitialize for Atomic SubSystem: '<S3>/Baro_Process' */
      /* InitializeConditions for Delay: '<S279>/Delay' */
      INS_DWork.Delay_DSTATE_f = INS_P.Delay_InitialCondition_n;
  
      /* InitializeConditions for UnitDelay: '<S287>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_g = INS_P.DetectChange_vinit_d;
  
      /* InitializeConditions for DiscreteIntegrator: '<S279>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ev =
        INS_P.DiscreteTimeIntegrator_IC_dv;
  
      /* SystemInitialize for Enabled SubSystem: '<S277>/Reference_and_Derivative' */
      /* InitializeConditions for Delay: '<S280>/Delay' */
      INS_DWork.Delay_DSTATE_d = INS_P.Delay_InitialCondition_p;
  
      /* InitializeConditions for Delay: '<S280>/Delay1' */
      INS_DWork.Delay1_DSTATE_i = INS_P.Delay1_InitialCondition_k;
  
      /* InitializeConditions for Delay: '<S280>/Delay2' */
      INS_DWork.Delay2_DSTATE = INS_P.Delay2_InitialCondition;
  
      /* SystemInitialize for Outport: '<S280>/pressure_R' */
      INS_B.Divide = INS_P.pressure_R_Y0;
  
      /* SystemInitialize for Outport: '<S280>/dh_dp' */
      INS_B.gain = INS_P.dh_dp_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S277>/Reference_and_Derivative' */
      /* End of SystemInitialize for SubSystem: '<S3>/Baro_Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<S3>/GPS_Process' */
      /* InitializeConditions for UnitDelay: '<S304>/Unit Delay' */
      INS_DWork.UnitDelay_DSTATE = INS_P.UnitDelay_InitialCondition;
  
      /* InitializeConditions for UnitDelay: '<S306>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_i = INS_P.DetectChange_vinit_e;
  
      /* InitializeConditions for DiscreteIntegrator: '<S300>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_i =
        INS_P.DiscreteTimeIntegrator_IC_p;
  
      /* SystemInitialize for Triggered SubSystem: '<S271>/WGS84_Model' */
      /* SystemInitialize for Outport: '<S294>/lon_0_rad' */
      INS_B.Scalefactor = INS_P.lon_0_rad_Y0;
  
      /* SystemInitialize for Outport: '<S294>/lat_0_rad' */
      INS_B.Scalefactor1 = INS_P.lat_0_rad_Y0;
  
      /* SystemInitialize for Outport: '<S294>/dx_dlat' */
      INS_B.Add = INS_P.dx_dlat_Y0;
  
      /* SystemInitialize for Outport: '<S294>/dy_dlon' */
      INS_B.Product4 = INS_P.dy_dlon_Y0;
  
      /* SystemInitialize for Outport: '<S294>/dz_dh' */
      INS_B.OutportBufferFordz_dh = INS_P.dz_dh_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S271>/WGS84_Model' */
      /* End of SystemInitialize for SubSystem: '<S3>/GPS_Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<S3>/IMU_Process' */
      /* InitializeConditions for UnitDelay: '<S321>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_h = INS_P.DetectChange_vinit_md;
  
      /* InitializeConditions for DiscreteIntegrator: '<S310>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_lp =
        INS_P.DiscreteTimeIntegrator_IC_b;
  
      /* InitializeConditions for UnitDelay: '<S327>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_ht = INS_P.DetectChange_vinit_j;
  
      /* InitializeConditions for DiscreteIntegrator: '<S312>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_d =
        INS_P.DiscreteTimeIntegrator_IC_pw;
  
      /* InitializeConditions for DiscreteIntegrator: '<S316>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_IC_LOAD = 1U;
  
      /* InitializeConditions for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE[0] =
        INS_P.DiscreteTimeIntegrator_IC_c;
  
      /* InitializeConditions for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_c[0] =
        INS_P.DiscreteTimeIntegrator_IC_n;
  
      /* InitializeConditions for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE[1] =
        INS_P.DiscreteTimeIntegrator_IC_c;
  
      /* InitializeConditions for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_c[1] =
        INS_P.DiscreteTimeIntegrator_IC_n;
  
      /* InitializeConditions for DiscreteIntegrator: '<S316>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE[2] =
        INS_P.DiscreteTimeIntegrator_IC_c;
  
      /* InitializeConditions for DiscreteIntegrator: '<S317>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_c[2] =
        INS_P.DiscreteTimeIntegrator_IC_n;
  
      /* InitializeConditions for DiscreteIntegrator: '<S317>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_IC_LO_a = 1U;
  
      /* End of SystemInitialize for SubSystem: '<S3>/IMU_Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<S3>/Mag_Process' */
      /* InitializeConditions for UnitDelay: '<S335>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_n = INS_P.DetectChange_vinit_l;
  
      /* InitializeConditions for DiscreteIntegrator: '<S331>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_l =
        INS_P.DiscreteTimeIntegrator_IC_k;
  
      /* End of SystemInitialize for SubSystem: '<S3>/Mag_Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<S3>/Optical_Flow_Preprocessing' */
      /* InitializeConditions for UnitDelay: '<S352>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_l = INS_P.DetectChange_vinit_i;
  
      /* InitializeConditions for DiscreteIntegrator: '<S350>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_n =
        INS_P.DiscreteTimeIntegrator_IC_g;
  
      /* End of SystemInitialize for SubSystem: '<S3>/Optical_Flow_Preprocessing' */
  
      /* SystemInitialize for Atomic SubSystem: '<S3>/Sonar_Process' */
      /* InitializeConditions for UnitDelay: '<S356>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE = INS_P.DetectChange_vinit_k;
  
      /* InitializeConditions for DiscreteIntegrator: '<S353>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kn =
        INS_P.DiscreteTimeIntegrator_IC_pt;
  
      /* InitializeConditions for Delay: '<S353>/Delay' */
      INS_DWork.Delay_DSTATE_dz = INS_P.Delay_InitialCondition_cg;
  
      /* End of SystemInitialize for SubSystem: '<S3>/Sonar_Process' */
      /* End of SystemInitialize for SubSystem: '<Root>/Sensor_Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<Root>/Data_Fusion' */
      for (i = 0; i < 9; i++) {
        /* InitializeConditions for Delay: '<Root>/Delay' */
        INS_DWork.Delay_6_DSTATE[i] = INS_P.Delay_6_InitialCondition;
        INS_DWork.Delay_8_DSTATE[i] = INS_P.Delay_8_InitialCondition_m;
  
        /* InitializeConditions for Delay: '<S56>/Delay' */
        INS_DWork.Delay_8_DSTATE_l[i] = INS_P.Delay_8_InitialCondition;
        INS_DWork.Delay_9_DSTATE[i] = INS_P.Delay_9_InitialCondition;
      }
  
      /* InitializeConditions for Delay: '<S56>/Delay' */
      INS_DWork.Delay_10_DSTATE[0] = INS_P.Delay_10_InitialCondition;
      INS_DWork.Delay_10_DSTATE[1] = INS_P.Delay_10_InitialCondition;
      INS_DWork.Delay_10_DSTATE[2] = INS_P.Delay_10_InitialCondition;
  
      /* InitializeConditions for Delay: '<S57>/Delay' */
      INS_DWork.Delay_9_DSTATE_m = INS_P.Delay_9_InitialCondition_h;
      INS_DWork.Delay_10_DSTATE_a = INS_P.Delay_10_InitialCondition_h;
      INS_DWork.Delay_11_DSTATE = INS_P.Delay_11_InitialCondition;
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Attitude_Init' */
      /* InitializeConditions for Delay: '<S109>/Delay' */
      INS_DWork.Delay_DSTATE_i = INS_P.Delay_InitialCondition_n0;
  
      /* InitializeConditions for UnitDelay: '<S112>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_p = INS_P.DetectRisePositive_vinit_fc;
  
      /* InitializeConditions for DiscreteIntegrator: '<S106>/Discrete-Time Integrator5' */
      INS_DWork.DiscreteTimeIntegrator5_DSTATE[0] =
        INS_P.DiscreteTimeIntegrator5_IC;
      INS_DWork.DiscreteTimeIntegrator5_DSTATE[1] =
        INS_P.DiscreteTimeIntegrator5_IC;
  
      /* InitializeConditions for DiscreteIntegrator: '<S104>/Discrete-Time Integrator5' */
      INS_DWork.DiscreteTimeIntegrator5_DSTAT_o =
        INS_P.DiscreteTimeIntegrator5_IC_m;
  
      /* InitializeConditions for DiscreteIntegrator: '<S105>/Discrete-Time Integrator5' */
      INS_DWork.DiscreteTimeIntegrator5_DSTAT_n =
        INS_P.DiscreteTimeIntegrator5_IC_a;
  
      /* SystemInitialize for Enabled SubSystem: '<S58>/Attitude_Init' */
      /* SystemInitialize for Merge: '<S65>/Merge' */
      INS_B.Merge[0] = INS_P.Merge_InitialOutput[0];
  
      /* SystemInitialize for Outport: '<S61>/quaternion_0' */
      INS_B.OutportBufferForquaternion_0[0] = INS_P.quaternion_0_Y0[0];
  
      /* SystemInitialize for Merge: '<S65>/Merge' */
      INS_B.Merge[1] = INS_P.Merge_InitialOutput[1];
  
      /* SystemInitialize for Outport: '<S61>/quaternion_0' */
      INS_B.OutportBufferForquaternion_0[1] = INS_P.quaternion_0_Y0[1];
  
      /* SystemInitialize for Merge: '<S65>/Merge' */
      INS_B.Merge[2] = INS_P.Merge_InitialOutput[2];
  
      /* SystemInitialize for Outport: '<S61>/quaternion_0' */
      INS_B.OutportBufferForquaternion_0[2] = INS_P.quaternion_0_Y0[2];
  
      /* SystemInitialize for Merge: '<S65>/Merge' */
      INS_B.Merge[3] = INS_P.Merge_InitialOutput[3];
  
      /* SystemInitialize for Outport: '<S61>/quaternion_0' */
      INS_B.OutportBufferForquaternion_0[3] = INS_P.quaternion_0_Y0[3];
  
      /* End of SystemInitialize for SubSystem: '<S58>/Attitude_Init' */
      /* End of SystemInitialize for SubSystem: '<S56>/Attitude_Init' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Correct' */
      /* InitializeConditions for DiscreteIntegrator: '<S138>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_IC_LO_o = 1U;
  
      /* InitializeConditions for Delay: '<S122>/Delay' */
      for (i = 0; i < 600; i++) {
        INS_DWork.Delay_DSTATE_lv[i] = INS_P.Delay_InitialCondition_m;
      }
  
      /* End of InitializeConditions for Delay: '<S122>/Delay' */
  
      /* InitializeConditions for UnitDelay: '<S139>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_ff = INS_P.DetectIncrease_vinit;
  
      /* InitializeConditions for DiscreteFir: '<S124>/Discrete FIR Filter' */
      INS_DWork.DiscreteFIRFilter_circBuf = 0;
      for (i = 0; i < 29; i++) {
        INS_DWork.DiscreteFIRFilter_states[i] =
          INS_P.DiscreteFIRFilter_InitialStates;
      }
  
      /* End of InitializeConditions for DiscreteFir: '<S124>/Discrete FIR Filter' */
  
      /* InitializeConditions for RateLimiter: '<S150>/Rate Limiter' */
      INS_DWork.PrevY = INS_P.RateLimiter_IC;
  
      /* SystemInitialize for Enabled SubSystem: '<S123>/GPS_Acc_Observer' */
      /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_IC_L_oi = 1U;
      INS_DWork.DiscreteTimeIntegrator1_PrevRes = 0;
  
      /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_PrevRe_c = 0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/GPS_Acc_Observer' */
  
      /* InitializeConditions for DiscreteIntegrator: '<S138>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[0] =
        INS_P.DiscreteTimeIntegrator_IC_d;
  
      /* SystemInitialize for Enabled SubSystem: '<S123>/GPS_Acc_Observer' */
      /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[0] =
        INS_P.DiscreteTimeIntegrator_IC_a;
  
      /* SystemInitialize for Outport: '<S140>/GPS_acc_O_mPs2' */
      INS_B.DiscreteTimeIntegrator[0] = INS_P.GPS_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/GPS_Acc_Observer' */
  
      /* InitializeConditions for DiscreteIntegrator: '<S138>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[1] =
        INS_P.DiscreteTimeIntegrator_IC_d;
  
      /* SystemInitialize for Enabled SubSystem: '<S123>/GPS_Acc_Observer' */
      /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[1] =
        INS_P.DiscreteTimeIntegrator_IC_a;
  
      /* SystemInitialize for Outport: '<S140>/GPS_acc_O_mPs2' */
      INS_B.DiscreteTimeIntegrator[1] = INS_P.GPS_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/GPS_Acc_Observer' */
  
      /* InitializeConditions for DiscreteIntegrator: '<S138>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_g[2] =
        INS_P.DiscreteTimeIntegrator_IC_d;
  
      /* SystemInitialize for Enabled SubSystem: '<S123>/GPS_Acc_Observer' */
      /* InitializeConditions for DiscreteIntegrator: '<S144>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_o[2] =
        INS_P.DiscreteTimeIntegrator_IC_a;
  
      /* SystemInitialize for Outport: '<S140>/GPS_acc_O_mPs2' */
      INS_B.DiscreteTimeIntegrator[2] = INS_P.GPS_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/GPS_Acc_Observer' */
  
      /* SystemInitialize for Enabled SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* InitializeConditions for DiscreteIntegrator: '<S145>/Discrete-Time Integrator1' */
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[0] =
        INS_P.DiscreteTimeIntegrator1_IC;
      INS_DWork.DiscreteTimeIntegrator1_DSTAT_b[1] =
        INS_P.DiscreteTimeIntegrator1_IC;
  
      /* InitializeConditions for DiscreteIntegrator: '<S145>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_IC_LOA_k = 1U;
  
      /* End of SystemInitialize for SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* End of SystemInitialize for SubSystem: '<S56>/Correct' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Integrate' */
      /* InitializeConditions for Delay: '<S153>/Delay' */
      INS_DWork.Delay_DSTATE_f0 = INS_P.Delay_InitialCondition_k;
  
      /* InitializeConditions for UnitDelay: '<S157>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_dy = INS_P.DetectIncrease_vinit_d;
  
      /* InitializeConditions for DiscreteIntegrator: '<S154>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_IC_LOADI = 1U;
      INS_DWork.DiscreteTimeIntegrator_PrevRese = 0;
  
      /* End of SystemInitialize for SubSystem: '<S56>/Integrate' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Correct' */
      /* SystemInitialize for Enabled SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* SystemInitialize for Outport: '<S142>/OptFlow_acc_O_mPs2' */
      INS_B.Reshape[0] = INS_P.OptFlow_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* End of SystemInitialize for SubSystem: '<S56>/Correct' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Integrate' */
      /* InitializeConditions for DiscreteIntegrator: '<S155>/Discrete-Time Integrator2' */
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[0] =
        INS_P.DiscreteTimeIntegrator2_IC;
  
      /* End of SystemInitialize for SubSystem: '<S56>/Integrate' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Correct' */
      /* SystemInitialize for Enabled SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* SystemInitialize for Outport: '<S142>/OptFlow_acc_O_mPs2' */
      INS_B.Reshape[1] = INS_P.OptFlow_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* End of SystemInitialize for SubSystem: '<S56>/Correct' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Integrate' */
      /* InitializeConditions for DiscreteIntegrator: '<S155>/Discrete-Time Integrator2' */
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[1] =
        INS_P.DiscreteTimeIntegrator2_IC;
  
      /* End of SystemInitialize for SubSystem: '<S56>/Integrate' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Correct' */
      /* SystemInitialize for Enabled SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* SystemInitialize for Outport: '<S142>/OptFlow_acc_O_mPs2' */
      INS_B.Reshape[2] = INS_P.OptFlow_acc_O_mPs2_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S123>/OpticalFlow_Acc_Observer' */
      /* End of SystemInitialize for SubSystem: '<S56>/Correct' */
  
      /* SystemInitialize for Atomic SubSystem: '<S56>/Integrate' */
      /* InitializeConditions for DiscreteIntegrator: '<S155>/Discrete-Time Integrator2' */
      INS_DWork.DiscreteTimeIntegrator2_DSTATE[2] =
        INS_P.DiscreteTimeIntegrator2_IC;
  
      /* End of SystemInitialize for SubSystem: '<S56>/Integrate' */
  
      /* SystemInitialize for Atomic SubSystem: '<S57>/Process' */
      /* InitializeConditions for UnitDelay: '<S238>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_j = INS_P.DetectRisePositive_vinit_h;
  
      /* InitializeConditions for UnitDelay: '<S240>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_o = INS_P.DetectRisePositive_vinit_me;
  
      /* InitializeConditions for Delay: '<S243>/Delay1' */
      INS_DWork.Delay1_DSTATE = INS_P.Delay1_InitialCondition;
  
      /* InitializeConditions for UnitDelay: '<S245>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_dp = INS_P.DetectRisePositive3_vinit;
  
      /* InitializeConditions for Delay: '<S244>/Delay' */
      INS_DWork.Delay_DSTATE[0] = INS_P.Delay_InitialCondition;
  
      /* InitializeConditions for Delay: '<S244>/Delay1' */
      INS_DWork.Delay1_DSTATE_o[0] = INS_P.Delay1_InitialCondition_e;
  
      /* InitializeConditions for Delay: '<S244>/Delay' */
      INS_DWork.Delay_DSTATE[1] = INS_P.Delay_InitialCondition;
  
      /* InitializeConditions for Delay: '<S244>/Delay1' */
      INS_DWork.Delay1_DSTATE_o[1] = INS_P.Delay1_InitialCondition_e;
  
      /* InitializeConditions for UnitDelay: '<S247>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_m = INS_P.DetectRisePositive3_vinit_h;
  
      /* End of SystemInitialize for SubSystem: '<S57>/Process' */
  
      /* SystemInitialize for Atomic SubSystem: '<S57>/CF' */
      /* InitializeConditions for Delay: '<S218>/Delay' */
      INS_DWork.Delay_DSTATE_b[0] = INS_P.Delay_InitialCondition_mt;
      INS_DWork.Delay_DSTATE_b[1] = INS_P.Delay_InitialCondition_mt;
      INS_DWork.Delay_DSTATE_b[2] = INS_P.Delay_InitialCondition_mt;
  
      /* SystemInitialize for Merge: '<S220>/Merge' */
      INS_B.h_0 = INS_P.Merge_InitialOutput_j;
  
      /* SystemInitialize for Merge: '<S220>/Merge1' */
      INS_B.vel_D_0 = INS_P.Merge1_InitialOutput;
  
      /* SystemInitialize for Enabled SubSystem: '<S198>/Baro_Correct' */
      /* InitializeConditions for UnitDelay: '<S201>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_lg = INS_P.DetectChange_vinit_m;
  
      /* InitializeConditions for Delay: '<S204>/x_delay' */
      INS_DWork.icLoad_a = 1U;
  
      /* InitializeConditions for UnitDelay: '<S208>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_f = INS_P.DetectIncrease_vinit_c;
  
      /* InitializeConditions for Delay: '<S206>/Delay' */
      INS_DWork.Delay_DSTATE_h = INS_P.Delay_InitialCondition_mk;
  
      /* End of SystemInitialize for SubSystem: '<S198>/Baro_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S199>/GPS_Correct' */
      /* InitializeConditions for UnitDelay: '<S210>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_d = INS_P.DetectChange_vinit_h;
  
      /* InitializeConditions for Delay: '<S213>/h_delay' */
      INS_DWork.icLoad = 1U;
  
      /* InitializeConditions for Delay: '<S213>/vd_delay' */
      INS_DWork.icLoad_f = 1U;
  
      /* InitializeConditions for UnitDelay: '<S217>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_ib = INS_P.DetectIncrease_vinit_j;
  
      /* InitializeConditions for Memory: '<S215>/Memory' */
      INS_DWork.Memory_PreviousInput = INS_P.Memory_InitialCondition_p;
  
      /* End of SystemInitialize for SubSystem: '<S199>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S198>/Baro_Correct' */
      /* InitializeConditions for Delay: '<S202>/Delay' */
      INS_DWork.Delay_DSTATE_dr[0] = INS_P.Delay_InitialCondition_j;
  
      /* SystemInitialize for Outport: '<S200>/X_Next' */
      INS_B.Sum1_p[0] = INS_P.X_Next_Y0_m;
  
      /* End of SystemInitialize for SubSystem: '<S198>/Baro_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S199>/GPS_Correct' */
      /* InitializeConditions for Memory: '<S211>/Memory' */
      INS_DWork.Memory_PreviousInput_e[0] = INS_P.Memory_InitialCondition_c;
  
      /* SystemInitialize for Outport: '<S209>/X_Next' */
      INS_B.Sum1[0] = INS_P.X_Next_Y0_i;
  
      /* End of SystemInitialize for SubSystem: '<S199>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S198>/Baro_Correct' */
      /* InitializeConditions for Delay: '<S202>/Delay' */
      INS_DWork.Delay_DSTATE_dr[1] = INS_P.Delay_InitialCondition_j;
  
      /* SystemInitialize for Outport: '<S200>/X_Next' */
      INS_B.Sum1_p[1] = INS_P.X_Next_Y0_m;
  
      /* End of SystemInitialize for SubSystem: '<S198>/Baro_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S199>/GPS_Correct' */
      /* InitializeConditions for Memory: '<S211>/Memory' */
      INS_DWork.Memory_PreviousInput_e[1] = INS_P.Memory_InitialCondition_c;
  
      /* SystemInitialize for Outport: '<S209>/X_Next' */
      INS_B.Sum1[1] = INS_P.X_Next_Y0_i;
  
      /* End of SystemInitialize for SubSystem: '<S199>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S198>/Baro_Correct' */
      /* InitializeConditions for Delay: '<S202>/Delay' */
      INS_DWork.Delay_DSTATE_dr[2] = INS_P.Delay_InitialCondition_j;
  
      /* SystemInitialize for Outport: '<S200>/X_Next' */
      INS_B.Sum1_p[2] = INS_P.X_Next_Y0_m;
  
      /* End of SystemInitialize for SubSystem: '<S198>/Baro_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S199>/GPS_Correct' */
      /* InitializeConditions for Memory: '<S211>/Memory' */
      INS_DWork.Memory_PreviousInput_e[2] = INS_P.Memory_InitialCondition_c;
  
      /* SystemInitialize for Outport: '<S209>/X_Next' */
      INS_B.Sum1[2] = INS_P.X_Next_Y0_i;
  
      /* End of SystemInitialize for SubSystem: '<S199>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S176>/GPS_Correct' */
      /* InitializeConditions for UnitDelay: '<S179>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_d0 = INS_P.DetectChange_vinit;
  
      /* InitializeConditions for Delay: '<S182>/x_delay' */
      INS_DWork.icLoad_o = 1U;
  
      /* InitializeConditions for Delay: '<S182>/x_delay1' */
      INS_DWork.icLoad_i = 1U;
  
      /* InitializeConditions for Delay: '<S182>/vn_delay' */
      INS_DWork.icLoad_m = 1U;
  
      /* InitializeConditions for Delay: '<S182>/vn_delay1' */
      INS_DWork.icLoad_j = 1U;
  
      /* InitializeConditions for UnitDelay: '<S187>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_gn = INS_P.DetectIncrease_vinit_p;
  
      /* End of SystemInitialize for SubSystem: '<S176>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S191>/GPS_Pos_Init' */
      /* SystemInitialize for Outport: '<S193>/X_0' */
      INS_B.DataTypeConversion[0] = INS_P.X_0_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S191>/GPS_Pos_Init' */
  
      /* SystemInitialize for Enabled SubSystem: '<S176>/GPS_Correct' */
      /* InitializeConditions for Memory: '<S185>/Memory' */
      INS_DWork.Memory_PreviousInput_d[0] = INS_P.Memory_InitialCondition[0];
  
      /* End of SystemInitialize for SubSystem: '<S176>/GPS_Correct' */
  
      /* SystemInitialize for Enabled SubSystem: '<S191>/GPS_Pos_Init' */
      /* SystemInitialize for Outport: '<S193>/X_0' */
      INS_B.DataTypeConversion[1] = INS_P.X_0_Y0;
  
      /* End of SystemInitialize for SubSystem: '<S191>/GPS_Pos_Init' */
  
      /* SystemInitialize for Enabled SubSystem: '<S176>/GPS_Correct' */
      /* InitializeConditions for Memory: '<S185>/Memory' */
      INS_DWork.Memory_PreviousInput_d[1] = INS_P.Memory_InitialCondition[1];
      for (i = 0; i < 6; i++) {
        /* InitializeConditions for Delay: '<S188>/Delay' */
        INS_DWork.Delay_DSTATE_l[i] = INS_P.Delay_InitialCondition_g;
  
        /* InitializeConditions for Memory: '<S180>/Memory' */
        INS_DWork.Memory_PreviousInput_h[i] = INS_P.Memory_InitialCondition_o;
  
        /* SystemInitialize for Outport: '<S178>/X_Next' */
        INS_B.Sum1_o[i] = INS_P.X_Next_Y0;
      }
  
      /* End of SystemInitialize for SubSystem: '<S176>/GPS_Correct' */
      /* End of SystemInitialize for SubSystem: '<S57>/CF' */
      /* End of SystemInitialize for SubSystem: '<Root>/Data_Fusion' */
  
      /* SystemInitialize for Atomic SubSystem: '<Root>/Bus_Assignment' */
      /* SystemInitialize for Atomic SubSystem: '<S1>/Counter' */
      /* InitializeConditions for UnitDelay: '<S4>/Output' */
      INS_DWork.Output_DSTATE = INS_P.Output_InitialCondition;
  
      /* End of SystemInitialize for SubSystem: '<S1>/Counter' */
  
      /* SystemInitialize for Atomic SubSystem: '<S1>/Status_Output' */
      /* InitializeConditions for DiscreteIntegrator: '<S13>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_b = INS_P.DiscreteTimeIntegrator_IC;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_n = 0;
  
      /* InitializeConditions for Delay: '<S13>/Delay' */
      INS_DWork.Delay_DSTATE_bn = INS_P.Delay_InitialCondition_go;
  
      /* InitializeConditions for DiscreteIntegrator: '<S16>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_h =
        INS_P.DiscreteTimeIntegrator_IC_i;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_m = 0;
  
      /* InitializeConditions for Delay: '<S16>/Delay' */
      INS_DWork.Delay_DSTATE_c = INS_P.Delay_InitialCondition_mz;
  
      /* InitializeConditions for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_b3 =
        INS_P.DiscreteTimeIntegrator_IC_o;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_d = 0;
  
      /* InitializeConditions for Delay: '<S18>/Delay' */
      INS_DWork.Delay_DSTATE_l0 = INS_P.Delay_InitialCondition_a;
  
      /* InitializeConditions for DiscreteIntegrator: '<S12>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_ca =
        INS_P.DiscreteTimeIntegrator_IC_l;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_h = 0;
  
      /* InitializeConditions for Delay: '<S12>/Delay' */
      INS_DWork.Delay_DSTATE_cn = INS_P.Delay_InitialCondition_c;
  
      /* InitializeConditions for DiscreteIntegrator: '<S14>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_e =
        INS_P.DiscreteTimeIntegrator_IC_j;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_a = 0;
  
      /* InitializeConditions for Delay: '<S14>/Delay' */
      INS_DWork.Delay_DSTATE_p = INS_P.Delay_InitialCondition_cj;
  
      /* InitializeConditions for DiscreteIntegrator: '<S15>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTAT_kg =
        INS_P.DiscreteTimeIntegrator_IC_ig;
      INS_DWork.DiscreteTimeIntegrator_PrevRe_o = 0;
  
      /* InitializeConditions for Delay: '<S15>/Delay' */
      INS_DWork.Delay_DSTATE_db = INS_P.Delay_InitialCondition_ae;
  
      /* InitializeConditions for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' */
      INS_DWork.DiscreteTimeIntegrator_DSTATE_a =
        INS_P.DiscreteTimeIntegrator_IC_e;
      INS_DWork.DiscreteTimeIntegrator_PrevR_ar = 0;
  
      /* InitializeConditions for Delay: '<S17>/Delay' */
      INS_DWork.Delay_DSTATE_ly = INS_P.Delay_InitialCondition_e;
  
      /* InitializeConditions for UnitDelay: '<S22>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_dx = INS_P.DetectRisePositive_vinit;
  
      /* InitializeConditions for UnitDelay: '<S27>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_mu = INS_P.DetectRisePositive_vinit_c;
  
      /* InitializeConditions for UnitDelay: '<S32>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_dq = INS_P.DetectRisePositive_vinit_m;
  
      /* InitializeConditions for UnitDelay: '<S37>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_f4 = INS_P.DetectRisePositive_vinit_k;
  
      /* InitializeConditions for UnitDelay: '<S42>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_ng = INS_P.DetectRisePositive_vinit_a;
  
      /* InitializeConditions for UnitDelay: '<S47>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_dql = INS_P.DetectRisePositive_vinit_f;
  
      /* InitializeConditions for UnitDelay: '<S52>/Delay Input1' */
      INS_DWork.DelayInput1_DSTATE_fq = INS_P.DetectRisePositive_vinit_l;
  
      /* End of SystemInitialize for SubSystem: '<S1>/Status_Output' */
      /* End of SystemInitialize for SubSystem: '<Root>/Bus_Assignment' */
    }
  }
  
  /*
   * File trailer for generated code.
   *
   * [EOF]
   */
  