LSQ filter

[gwbres]

Following the idea of nyx being our navigation backbone framework,
I'm trying to convert and propose my LSQ filter implementation for 3D+T navigation.

Side note, the "problem" I see with Nyx is that it is kind of overly generic:

1. I presume the dimensions we're dealing with remain U3, U4 or U6. Although, that may not apply to the filter API, because it is super interesting to propose a fully generic filter that other fields of application may use.

2. In the example of the existing KF, it is generic yet the API seems very tied to what a KF is and how it works.

The LSQ filter is very simple, it iteratively estimates the best correction to apply to the initial/current state. It is not a predictive filter. The Kalman filter was originally developped to enhance the LSQ filter. It is defined according to the following equations (for 3D+T in GNSS):

The H matrix, that I call h in my function, that you need to confirm is the H tilde in your notation, which is always squared and T::Size ? . For me, the LSQ requires State with the particularity that T::Size = T::VecLength, the squared matrix size and the vector length are identical:

The correction vector (T::VecLength ?) is obtained from the least square equation, from H and b the measurement + noise vector, which is also T::VecLength:

A state update is simply X + dX.

First you need to confirm that your H tilde and my H matrix are the same.
Also, confirm that b will eventually be generalized to "SNC", considering that in my simpler case it is a unique vector (not N matrices).
You will also have to tell me whether my iterate() function should go to time_update or measurement_update, I presume this is KF terminology, and I have no idea

[ChristopherRabotin]

Génial Guillaume ! Ça fait longtemps que Nyx a besoin d'un filtre de moindre carrés ! A première vue, le code me paraît bon. La différence que j'ai remarqué c'est que les filtres LSQ que j'ai vu pour la restitution d'orbite peuvent généralement ingester plusieurs (dizaines voire centaines) de mesures à la fois. J'ai plus de détails sur mon autre ordinateur, et je comparerai ton algo avec celui que je connais (et en anglais).

[gwbres]

j'ai remarqué c'est que les filtres LSQ que j'ai vu pour la restitution d'orbite peuvent généralement ingester plusieurs (dizaines voire centaines) de mesures à la fois [..]
J'ai plus de détails sur mon autre ordinateur, et je comparerai ton algo avec celui que je connais (et en anglais).

originellement on appelle iterate(), qui est pratiquement intégrée à measurement_update, autant de fois que nécessaire pour un état initial donné (c'est une boucle). Pour chaque Epoch on présente un état à l'entrée et on va itérer N fois, jusqu'à par exemple N=10 ou bien on sort quand un critère de convergence est atteint (ex: |dX| < 1e-6). A chaque sortie de boucle on obtient le nouvel état X* = X + dX .

Vu ce que tu me dis, tu n'as l'air de faire qu'une seule itération de KF toi classiquement ? en tous cas, je ne vois pas ce qui empecherait d'appeler l'API N fois, surtout si les covariances et vecteurs d'états sont publiques / accessibles, ce qui permet de décider si l'on continue ou pas.

[ChristopherRabotin]

Here is the algorithm as described by Tapley & Schulz:

Which follows the same nomenclature as in https://nyxspace.com/nyxspace/MathSpec/orbit_determination/kalman/#measurement-update. So H tilde maps the measurement (e.g. range and Doppler) to the state space (e.g. position and velocity), which is different from your H matrix. Then the \Phi matrix in the screenshots above is retrieved by the stm() function call: https://docs.rs/nyx-space/latest/nyx_space/cosmic/struct.Spacecraft.html#method.stm . That call will only return Ok if the state was propagated after the copied state was initialized using the with_stm() call, cf. https://docs.rs/nyx-space/latest/nyx_space/cosmic/struct.Spacecraft.html#method.with_stm . However, there is a difference: the stm returned from the stm() call is the $\Phi(t_i, t_{i+1})$, but the Batch Least Squares needs the $\Phi(t_0, t_{i})$. Luckily, the linearization process means that $\Phi(t_0, t_{i+1}) = \Phi(t_0, t_i) * \Phi(t_i, t_{i+1})$, so we just need to keep a copy of the previous STM, and append it to a large matrix.

I think that this SE question uses the equations you are used to with the nomenclature I usually use: https://math.stackexchange.com/questions/3243798/batch-least-squares .

Concerning how generic the state is, yes, that's on purpose. It allows for example to quickly switch between a filter that processes range and Doppler simultaneously or sequentially, and also estimating the solar radiation pressure coefficient, or changing the dynamics entirely and running an attitude/orientation filter with N star trackers for example. The mathematics are equivalent, so I wanted the implementation to be generic over any size. I agree with you that the Filter trait is possibly too close to a Kalman filter approach compared to a BLSE; in version 1.0 of Nyx, there was a square root information filter that worked well with this trait definition however.

Good luck, let me know how I can help!