openINS USBL Integration

Below are some BASIC (see the emphasis on "basic") notes on how USBL is integrated into our INS application.

• Our main INS loop is propagated by the reception of the IMU data
  • Every time we get IMU data the USBL sub task is run
• The USBL sub task conditions and filters the USBL data alone, ie does not consider any other sensor.
• General notes on USBL sub task (A bit more detailed in the diagram)
  • Check is we have new USBL data
  • Apply a velocity model to the USBL data and check the data fits (if it does not fix we do not accept the data point)
  • If the velocity has not been OK for a set number of measurements go forward with using the data (the intention here is to not get stuck using old data if we have continuous bad data and we are simply not passing the velocity models parameters)
  • Derive data from the USBL (ie state information)
  • Check if the USBL data is stable
  • If the data is stable use the stability point as the USBL sub-tasks initial fix
• General notes on the USBL Flow (This only describes the relevant function for USBL Accel mode)
  • run the USBL sub task:
    • Get if the sub task has determined the USBL is OK
    • Get the last valid USBL measurement
  • run the initial fix function to set the solution initial fix variables (all data is relative to this point)
    • If USBL is calculated as stable and it is the first time, the solution initial fix point will move to this point (User will experience a discontinuity in position (I think (How about this for the use of triple parentheses))).
  • If GPS is NOT OK and USBL is OK the run_usbl_newfix() will be called (This is not clear in the USBL Flow Diagram)
    • The run_usbl_newfix() handles all logic associated while running with an OK USBL including these modes of operation
      • IMU, USBL, DVL
      • IMU, USBL
      • USBL (Only when operating in the no IMU configuration)
    • When in the IMU, USBL mode
      • If it is the first time reset the KF
      • Run the KF (mixes accelerations with the USBL positions and velocities)
      • Get the state estimate: