Add least squares batch filter

[ChristopherRabotin]

High level description

Least squares batch filters and classical Kalman filters are both methods for estimating the state of a dynamic system from noisy measurements. They both use a mathematical model of the system to predict the state at future times and then use the measurements to correct the predictions and update the estimate of the state.

One advantage of a least squares batch filter is that it can provide a more accurate estimate of the state than a classical Kalman filter, especially when the measurements are correlated or the system is nonlinear. This is because the least squares method uses a least squares estimate to update the state, which is optimal in a least squares sense and takes into account the correlations between the measurements.

A disadvantage of a least squares batch filter is that it requires all of the measurements to be available at once in order to compute the optimal estimate of the state. This is known as a batch processing approach, and it is not suitable for systems that require a real-time estimate of the state. In contrast, a classical Kalman filter can update the estimate of the state incrementally as new measurements become available, using a recursive algorithm that is more computationally efficient than the batch approach.

Another disadvantage of a least squares batch filter is that it can be more sensitive to model errors and outliers in the measurements than a classical Kalman filter. This is because the least squares method assumes that the model and measurements are correct, and any errors or outliers in the data can bias the estimate of the state. In contrast, a classical Kalman filter can use a process noise covariance matrix to account for model errors and a measurement noise covariance matrix to downweight the influence of outliers on the estimate of the state.

---

The Levenberg-Marquardt optimization algorithm is a method for solving nonlinear least squares problems, which are optimization problems where the objective function is the sum of squares of nonlinear functions. This type of optimization problem is commonly used in state estimation, where the objective function is the sum of squares of the residuals between the measurements and the model predictions.

A Levenberg-Marquardt optimization algorithm can be used to solve a least squares batch filter problem, where the objective function is the sum of squares of the residuals between the measurements and the model predictions at the current estimate of the state. The algorithm can iteratively update the estimate of the state by computing the Jacobian matrix of the residuals with respect to the state variables and using this Jacobian matrix to compute a search direction for the update. The algorithm can then compute the optimal step size along this search direction using the Levenberg-Marquardt damping factor, which balances the tradeoff between the accuracy of the estimate and the convergence of the algorithm.

In summary, a Levenberg-Marquardt optimization algorithm can be used to solve a least squares batch filter problem, but it is not the only method that can be used. Other methods, such as the Gauss-Newton method or the trust-region method, can also be used to solve this type of optimization problem.

Requirements

• Nyx shall support least squares batch filtering for state estimation.
• Nyx shall provide an interface for specifying the mathematical model of the system and the measurement noise covariance matrix.
• Nyx shall provide functions for initializing the filter and adding measurements to the filter.
• Nyx shall provide functions for computing the optimal estimate of the state and the covariance matrix of the estimate.
• Nyx shall provide functions for updating the filter with new measurements and for resetting the filter to a new initial state.
• Nyx shall provide an example implementation of a least squares batch filter for a linear system with uncorrelated measurement noise.

Test plans

• Test the interface for specifying the mathematical model of the system and the measurement noise covariance matrix. This should include testing the input validation for the model and the covariance matrix, as well as testing the correctness of the model and covariance matrix when used in the filter.
• Test the functions for initializing the filter and adding measurements to the filter. This should include testing the input validation for the initial state and the measurements, as well as testing the correct initialization and updating of the filter state.
• Test the functions for computing the optimal estimate of the state and the covariance matrix of the estimate. This should include testing the correctness of the estimates and the covariance matrix, as well as testing the sensitivity of the estimates to the initial state and the measurements.
• Test the functions for updating the filter with new measurements and for resetting the filter to a new initial state. This should include testing the correct updating and resetting of the filter state, as well as testing the stability and convergence of the filter when applied to a variety of test cases.

Edge cases

• Test the behavior of the filter when the model or the covariance matrix are incorrect or singular. This should include testing the sensitivity of the filter to errors in the model or the covariance matrix, as well as testing the robustness of the filter to outliers or missing measurements.
• Test the behavior of the filter when the initial state or the measurements are noisy or biased. This should include testing the sensitivity of the filter to errors in the initial state or the measurements, as well as testing the ability of the filter to correct these errors over time.
• Test the performance of the filter when applied to large or complex systems, with many state variables and measurements. This should include testing the computational efficiency and numerical stability of the filter, as well as testing the accuracy of the estimates and the convergence of the algorithm.

Design

This is the design section. Each subsection has its own subsection in the quality assurance document.

API definition

Define how the Nyx APIs will be affect by this: what are new functions available, do any previous function change their definition, why call these functions by that name, etc.

High level architecture

Human note: this does not seem correct. ChatGPT is recommending using a mix of Levenberg Marquart and a LSBF.

The filter receives measurements from the system and uses the mathematical model of the system to predict the state at future times. The filter then updates the estimate of the state using the measurements and the covariance matrix, and computes the objective function as the sum of squares of the residuals between the measurements and the model predictions. The filter can then iterate on this process, using the Jacobian matrix of the residuals to compute a search direction for the update and the Levenberg-Marquardt damping factor to compute the optimal step size along this search direction. The filter can continue to update the estimate of the state until the objective function reaches a minimum or the algorithm converges.

Detailed design

The detailed design *will be used in the documentation of how Nyx works.

Feel free to fill out additional QA sections here, but these will typically be determined during the development, including the release in which this issue will be tackled.

[ChristopherRabotin]

BLS from Tapley:

[ChristopherRabotin]

Levenberg Marquart does not work well with a single noisy measurement. I guess that it isn't surprising if it only has a single measurement, so I need to implement the multiple measurement approach. That would be quite similar to a batch least squares, and the size of the Jacobian would be variable, based on the number of measurements. The LM crate only supports fixed sizes, so I may just implement the BLS approach using a smarter optimization approach like one from arg-min, scheduled in #119 .

[gwbres]

Hello Chris, not sure this is relevant here, but anyways.
In gnss-rtk we can deploy a recursive least square solver, currently if the solving method is set to Mode::LSQSPP.
From now on, all new (more advanced) solving methods will be recursive, while the initial Mode::SPP is not (we remain in the None hand side of that function).

My implementation is very tied to the process of solving a position.
Note that gnss-rtk will use nyx's implementation of the Kalman filter in a near future.

[ChristopherRabotin]

This work should also move the iterate_arc to the BLSE because iteration is only used there. Instead, the ODP should only have a smoother.