chore: Research Kalman Filtering as proposed by SquirrelKnight

[lsetiawan]

Overview

In PR https://github.com/seafloor-geodesy/gnatss/pull/167 @SquirrelKnight proposed kalman filtering method that need to be integrated to GNATSS. We need to see how we can integrate that in.

[madhavmk]

When I attempt to run the Kalman filtering code, there is an Exception raised.

Traceback (most recent call last):
  File "C:\Users\madha\AppData\Local\JetBrains\PyCharm Community Edition 2022.1.1\plugins\python-ce\helpers\pydev\pydevd.py", line 1491, in _exec
    pydev_imports.execfile(file, globals, locals)  # execute the script
  File "C:\Users\madha\AppData\Local\JetBrains\PyCharm Community Edition 2022.1.1\plugins\python-ce\helpers\pydev\_pydev_imps\_pydev_execfile.py", line 18, in execfile
    exec(compile(contents+"\n", file, 'exec'), glob, loc)
  File "D:/SSEC/offshore-geodesy/kalman.py", line 373, in <module>
    x, P, K, Pp = run_filter_simulation(df)
  File "D:/SSEC/offshore-geodesy/kalman.py", line 302, in run_filter_simulation
    gnss_kf.updatePosition(row)
  File "D:/SSEC/offshore-geodesy/kalman.py", line 211, in updatePosition
    K = (self.P @ H.transpose()) @ inv(S)
  File "C:\ProgramData\Miniconda3\envs\gnatss\lib\site-packages\numpy\linalg\linalg.py", line 561, in inv
    ainv = _umath_linalg.inv(a, signature=signature, extobj=extobj)
  File "C:\ProgramData\Miniconda3\envs\gnatss\lib\site-packages\numpy\linalg\linalg.py", line 112, in _raise_linalgerror_singular
    raise LinAlgError("Singular matrix")
numpy.linalg.LinAlgError: Singular matrix

This occurs when iterating through the 32nd row in the dataframe. We reach the position to run the Kalman Filter's updatePosition() method. In this method we attemp to invert the matrix

K = (self.P @ H.transpose()) @ inv(S)

This fails because S is a non-invertible matrix due to having a Determinant of 0. This leads to LinAlgError Exception

S = [
[0.00221376 0.00151327 0.00108549], 
[0.00148968 0.00101831 0.00073045], 
[0.00113651 0.00077689 0.00055728]
]

[madhavmk]

Hello @lsetiawan , I investigated the Kalman workflow, and discovered a bug as described above. You could reproduce the bug in this Google Colab Notebook I shared with you. Do let me know your thoughts 😄

[madhavmk]

I experimented and discovered this Issue was caused by duplicate rows in the merged dataframe. For example Index 30-32 in figure below.

I fixed this by removing duplicate dts rows in the vel_df, vel_std, pos_df dataframes. This prevented the merged dataframe from having multipe rows with same dts value like in figure above. The Kalman filter operation was successful after this modification.

I would really appreciate if @SquirrelKnight looked at this bug, and give his opinion on my bug-fix. Thank you 😃

[lsetiawan]

After some more investigation, @johnbdesanto has shed a light that the input file is the culprit for this repetitive timestamp, specifically the COV_VEL file. This file includes repetitive timestamp after a loss of floating point during a conversion from GPS Time to J2000 Seconds. However, there needs to be another investigation to ensure that this is truly the culprit, as I propose the following investigation:

1. Have different values for dts so that there are no repetitive timestamp
2. Create repeated values for lat,lon,hae,vel_n,vel_e,vel_u,x, y, z, sdx, sdy, sdz, sdxy, sdxz, sdyz
3. The values of the following columns should also vary by a slight bit v_sde, v_sdn, v_sdu

Please correct me if that's not a valid testing in your mind @johnbdesanto. Thanks!

[madhavmk]

The Kalman filter seems to be working when loading Novatel L1 data directly (and removing duplicate timestamps) My implementation can bee seen in PR #232

[madhavmk]

Colab Notebook version of working Kalman Filter: kalman_test.ipynb

[lsetiawan]

This took multiple sprints due to unexpected bugs that were found in the input data as well as the source code, but research has been completed.