| 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 KeyedLeastSquares.hpp | ||
| 10 | /// @brief Least squares with keyed states | ||
| 11 | /// @author P. Peitschat (HiWi) | ||
| 12 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 13 | /// @date 2023-09-26 | ||
| 14 | |||
| 15 | #pragma once | ||
| 16 | |||
| 17 | #include "util/Container/KeyedMatrix.hpp" | ||
| 18 | |||
| 19 | namespace NAV | ||
| 20 | { | ||
| 21 | /// @brief Least Squares Uncertainties return value | ||
| 22 | template<typename Scalar, typename StateKeyType> | ||
| 23 | struct KeyedLeastSquaresResult | ||
| 24 | { | ||
| 25 | KeyedVectorX<Scalar, StateKeyType> solution; ///< Least squares solution | ||
| 26 | KeyedMatrixX<Scalar, StateKeyType, StateKeyType> variance; ///< Least squares variance | ||
| 27 | }; | ||
| 28 | |||
| 29 | /// @brief Finds the "least squares" solution for the equation \f$ \mathbf{v} = \mathbf{dz} - \mathbf{H} \mathbf{x} \f$ | ||
| 30 | /// | ||
| 31 | /// Minimizes the functional (see LeastSquares.hpp) | ||
| 32 | /// @param[in] H Design Matrix | ||
| 33 | /// @param[in] dz Residual vector | ||
| 34 | /// @return Least squares solution | ||
| 35 | template<typename Scalar, typename StateKeyType, typename MeasKeyType> | ||
| 36 | KeyedVectorX<Scalar, StateKeyType> solveLinearLeastSquares(const KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>& H, const KeyedVectorX<Scalar, MeasKeyType>& dz) | ||
| 37 | { | ||
| 38 | KeyedVectorX<Scalar, StateKeyType> dx = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(H.cols()), H.colKeys()); | ||
| 39 | dx(all) = (H(all, all).transpose() * H(all, all)).inverse() * H(all, all).transpose() * dz(all); | ||
| 40 | return dx; | ||
| 41 | } | ||
| 42 | |||
| 43 | /// @brief Finds the "weighted least squares" solution (see LeastSquares.hpp) | ||
| 44 | /// @param[in] H Design Matrix | ||
| 45 | /// @param[in] W Weight matrix | ||
| 46 | /// @param[in] dz Residual vector | ||
| 47 | /// @return Least squares solution | ||
| 48 | template<typename Scalar, typename StateKeyType, typename MeasKeyType> | ||
| 49 | KeyedVectorX<Scalar, StateKeyType> solveWeightedLinearLeastSquares(const KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>& H, const KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>& W, const KeyedVectorX<Scalar, MeasKeyType>& dz) | ||
| 50 | { | ||
| 51 | KeyedVectorX<Scalar, StateKeyType> dx = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(H.cols()), H.colKeys()); | ||
| 52 | dx(all) = (H(all, all).transpose() * W(all, all) * H(all, all)).inverse() * H(all, all).transpose() * W(all, all) * dz(all); | ||
| 53 | return dx; | ||
| 54 | } | ||
| 55 | |||
| 56 | /// @brief Finds the "least squares" solution for the equation \f$ \mathbf{v} = \mathbf{dz} - \mathbf{H} \mathbf{x} \f$ | ||
| 57 | /// @param[in] H Design Matrix | ||
| 58 | /// @param[in] dz Residual vector | ||
| 59 | /// @return Least squares solution and variance | ||
| 60 | template<typename Scalar, typename StateKeyType, typename MeasKeyType> | ||
| 61 | KeyedLeastSquaresResult<Scalar, StateKeyType> | ||
| 62 | ✗ | solveLinearLeastSquaresUncertainties(const KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>& H, const KeyedVectorX<Scalar, MeasKeyType>& dz) | |
| 63 | { | ||
| 64 | // Amount of equations | ||
| 65 | ✗ | auto m = static_cast<int>(H.rows()); | |
| 66 | // Amount of variables | ||
| 67 | ✗ | auto n = static_cast<int>(H.cols()); | |
| 68 | |||
| 69 | // Cofactor matrix | ||
| 70 | ✗ | KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Q = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), H.colKeys(), H.colKeys()); | |
| 71 | ✗ | Q(all, all) = (H(all, all).transpose() * H(all, all)).inverse(); | |
| 72 | LOG_DATA("Q = \n{}", Q(all, all)); | ||
| 73 | // Least squares solution | ||
| 74 | ✗ | KeyedVectorX<Scalar, StateKeyType> dx = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(n), H.colKeys()); | |
| 75 | ✗ | dx(all) = Q(all, all) * H(all, all).transpose() * dz(all); | |
| 76 | LOG_DATA("dx = {}", dx(all).transpose()); | ||
| 77 | |||
| 78 | // Residual sum of squares | ||
| 79 | ✗ | double RSS = std::pow(dz(all).norm(), 2); | |
| 80 | LOG_DATA("RSS = {}", RSS); | ||
| 81 | |||
| 82 | // Statistical degrees of freedom | ||
| 83 | ✗ | auto dof = m - n; | |
| 84 | LOG_DATA("dof = {}", dof); | ||
| 85 | |||
| 86 | // Estimated error variance (reduced chi-squared statistic) | ||
| 87 | ✗ | double sigma2 = RSS / static_cast<double>(dof); | |
| 88 | LOG_DATA("sigma2 = {}", sigma2); | ||
| 89 | |||
| 90 | // Covariance matrix | ||
| 91 | ✗ | Q(all, all) *= sigma2; | |
| 92 | LOG_DATA("variance = \n{}", Q(all, all)); | ||
| 93 | |||
| 94 | ✗ | return { .solution = dx, .variance = Q }; | |
| 95 | ✗ | } | |
| 96 | |||
| 97 | /// @brief Finds the "weighted least squares" solution | ||
| 98 | /// @param[in] H Design Matrix | ||
| 99 | /// @param[in] W Weight matrix | ||
| 100 | /// @param[in] dz Residual vector | ||
| 101 | /// @return Weighted least squares solution and variance | ||
| 102 | template<typename Scalar, typename StateKeyType, typename MeasKeyType> | ||
| 103 | KeyedLeastSquaresResult<Scalar, StateKeyType> | ||
| 104 | 930 | solveWeightedLinearLeastSquaresUncertainties(const KeyedMatrixX<Scalar, MeasKeyType, StateKeyType>& H, const KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType>& W, const KeyedVectorX<Scalar, MeasKeyType>& dz) | |
| 105 | { | ||
| 106 | // Amount of equations | ||
| 107 | 930 | auto m = static_cast<int>(H.rows()); | |
| 108 | // Amount of variables | ||
| 109 | 930 | auto n = static_cast<int>(H.cols()); | |
| 110 | |||
| 111 | // Cofactor matrix | ||
| 112 |
2/4✓ Branch 3 taken 930 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 930 times.
✗ Branch 7 not taken.
|
930 | KeyedMatrixX<Scalar, StateKeyType, StateKeyType> Q = KeyedMatrixX<Scalar, StateKeyType, StateKeyType>(Eigen::MatrixX<Scalar>::Zero(n, n), H.colKeys(), H.colKeys()); |
| 113 |
5/10✓ Branch 4 taken 930 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 930 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 930 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 930 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 930 times.
✗ Branch 18 not taken.
|
930 | Q(all, all) = (H(all, all).transpose() * W(all, all) * H(all, all)).inverse(); |
| 114 | LOG_DATA("Q = \n{}", Q(all, all)); | ||
| 115 | |||
| 116 | // Least squares solution | ||
| 117 |
2/4✓ Branch 2 taken 930 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 930 times.
✗ Branch 6 not taken.
|
930 | KeyedVectorX<Scalar, StateKeyType> dx = KeyedVectorX<Scalar, StateKeyType>(Eigen::VectorX<Scalar>::Zero(n), H.colKeys()); |
| 118 |
5/10✓ Branch 4 taken 930 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 930 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 930 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 930 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 930 times.
✗ Branch 19 not taken.
|
930 | dx(all) = Q(all, all) * H(all, all).transpose() * W(all, all) * dz(all); |
| 119 | LOG_DATA("dx = {}", dx(all).transpose()); | ||
| 120 | |||
| 121 | // Residual sum of squares | ||
| 122 |
4/8✓ Branch 4 taken 930 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 930 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 930 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 930 times.
✗ Branch 14 not taken.
|
930 | double RSS = dz(all).transpose() * W(all, all) * dz(all); |
| 123 | LOG_DATA("RSS = {}", RSS); | ||
| 124 | |||
| 125 | // Statistical degrees of freedom | ||
| 126 | 930 | auto dof = m - n; | |
| 127 | LOG_DATA("dof = {}", dof); | ||
| 128 | |||
| 129 | // Estimated error variance (reduced chi-squared statistic) | ||
| 130 | 930 | double sigma2 = RSS / static_cast<double>(dof); | |
| 131 | LOG_DATA("sigma2 = {}", sigma2); | ||
| 132 | |||
| 133 | // Covariance matrix | ||
| 134 |
1/2✓ Branch 2 taken 930 times.
✗ Branch 3 not taken.
|
930 | Q(all, all) *= sigma2; |
| 135 | LOG_DATA("Covariance matrix = \n{}", Q(all, all)); | ||
| 136 | |||
| 137 | 930 | return { .solution = dx, .variance = Q }; | |
| 138 | 930 | } | |
| 139 | |||
| 140 | } // namespace NAV | ||
| 141 |