INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/internal/PolynomialRegressor/IncrementalLeastSquares.hpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 41 41 100.0%
Functions: 6 6 100.0%
Branches: 35 70 50.0%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file IncrementalLeastSquares.hpp
10 /// @brief Incremental Least Squares Curve Fit
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-10-26
13 /// @note See https://blog.demofox.org/2016/12/22/incremental-least-squares-curve-fitting/
14
15 #pragma once
16
17 #include <cstddef>
18 #include <vector>
19 #include <cmath>
20 #include "util/Assert.h"
21 #include "util/Eigen.hpp"
22
23 namespace NAV
24 {
25
26 /// @brief Incremental Least Squares Curve Fitting
27 /// @tparam Scalar Data type to store
28 template<typename Scalar>
29 class IncrementalLeastSquares
30 {
31 public:
32 /// @brief Constructor
33 /// @param[in] polynomialDegree Degree of the polynomial to fit
34 97520 explicit IncrementalLeastSquares(size_t polynomialDegree)
35 97520 {
36
1/2
✓ Branch 1 taken 97520 times.
✗ Branch 2 not taken.
 97520 setPolynomialDegree(polynomialDegree);
37 97520 }
38
39 /// @brief Set the Polynomial Degree
40 /// @param[in] polynomialDegree Degree of the polynomial to fit
41 195040 void setPolynomialDegree(size_t polynomialDegree)
42 {
43 195040 _polyDegree = polynomialDegree;
44
2/4
✓ Branch 1 taken 195040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195040 times.
✗ Branch 5 not taken.
 195040 _summedPowersX = Eigen::VectorX<Scalar>::Zero((static_cast<Eigen::Index>(polynomialDegree) + 1) * 2 - 1);
45
2/4
✓ Branch 1 taken 195040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195040 times.
✗ Branch 5 not taken.
 195040 _summedPowersXTimesY = Eigen::VectorX<Scalar>::Zero(static_cast<Eigen::Index>(polynomialDegree) + 1);
46 195040 }
47
48 /// @brief Add a data point to the polynomial
49 /// @param[in] x X Value
50 /// @param[in] y Y Value
51 1122705 void addDataPoint(const Scalar& x, const Scalar& y)
52 {
53 1122705 auto xpow = static_cast<Scalar>(1.0);
54
4/6
✓ Branch 1 taken 1122705 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1122705 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 5613525 times.
✓ Branch 9 taken 1122705 times.
 7858935 for (auto& sum : _summedPowersX)
55 {
56 5613525 sum += xpow;
57
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 5613525 xpow *= x;
58 }
59
60 1122705 xpow = static_cast<Scalar>(1.0);
61
4/6
✓ Branch 1 taken 1122705 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1122705 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3368115 times.
✓ Branch 9 taken 1122705 times.
 5613525 for (auto& sum : _summedPowersXTimesY)
62 {
63 3368115 sum += xpow * y;
64
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 3368115 xpow *= x;
65 }
66 1122705 }
67
68 /// @brief Removes a data point from the polynomial fit
69 /// @param[in] x X Value
70 /// @param[in] y Y Value
71 571 void removeDataPoint(const Scalar& x, const Scalar& y)
72 {
73 571 auto xpow = static_cast<Scalar>(1.0);
74
4/6
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 571 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2855 times.
✓ Branch 9 taken 571 times.
 3997 for (auto& sum : _summedPowersX)
75 {
76 2855 sum -= xpow;
77
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2855 xpow *= x;
78 }
79
80 571 xpow = static_cast<Scalar>(1.0);
81
4/6
✓ Branch 1 taken 571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 571 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1713 times.
✓ Branch 9 taken 571 times.
 2855 for (auto& sum : _summedPowersXTimesY)
82 {
83 1713 sum -= xpow * y;
84
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1713 xpow *= x;
85 }
86 571 }
87
88 /// @brief Reset the saved data
89 99830 void reset()
90 {
91 99830 _summedPowersX.setZero();
92 99830 _summedPowersXTimesY.setZero();
93 99830 }
94
95 /// @brief Calculates the polynomial coefficients in order a0 + a1 * x + a2 * x^2 + ...
96 12102 [[nodiscard]] Eigen::VectorX<Scalar> calcCoefficients() const
97 {
98
2/6
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 12102 if (_summedPowersX(0) == 0) { return {}; }
99
100
1/2
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
 12102 auto effectiveDegree = static_cast<Eigen::Index>(std::min(_polyDegree, static_cast<size_t>(_summedPowersX(0)) - 1));
101
102 // A^T * A matrix
103
1/2
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
 12102 Eigen::MatrixXd ATA(effectiveDegree + 1, effectiveDegree + 1);
104
2/2
✓ Branch 0 taken 36213 times.
✓ Branch 1 taken 12102 times.
 48315 for (Eigen::Index i = 0; i < effectiveDegree + 1; ++i)
105 {
106
3/6
✓ Branch 1 taken 36213 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36213 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36213 times.
✗ Branch 8 not taken.
 36213 ATA.row(i) = _summedPowersX.segment(i, effectiveDegree + 1);
107 }
108
109
5/10
✓ Branch 1 taken 12102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12102 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12102 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12102 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12102 times.
✗ Branch 14 not taken.
 12102 return ATA.inverse().topRows(effectiveDegree + 1) * _summedPowersXTimesY.head(effectiveDegree + 1);
110 12102 }
111
112 private:
113 size_t _polyDegree = 2; ///< Polynomial degree to fit
114 Eigen::VectorX<Scalar> _summedPowersX; ///< Sum{x^2}. (DEGREE + 1) * 2 - 1 values
115 Eigen::VectorX<Scalar> _summedPowersXTimesY; ///< Sum{x^2 * y}. DEGREE + 1 values
116 };
117
118 } // namespace NAV
119