| 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 LeastSquares.hpp | ||
| 10 | /// @brief Least Squares Algorithm | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-05-04 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <Eigen/Core> | ||
| 17 | #include <Eigen/Dense> | ||
| 18 | #include <unsupported/Eigen/MatrixFunctions> | ||
| 19 | #include <cmath> | ||
| 20 | #include "util/Logger.hpp" | ||
| 21 | |||
| 22 | namespace NAV | ||
| 23 | { | ||
| 24 | |||
| 25 | /// @brief Least Squares Uncertainties return value | ||
| 26 | template<typename SolutionVector, typename VarianceMatrix> | ||
| 27 | struct LeastSquaresResult | ||
| 28 | { | ||
| 29 | SolutionVector solution; ///< Least squares solution | ||
| 30 | VarianceMatrix variance; ///< Least squares variance | ||
| 31 | }; | ||
| 32 | |||
| 33 | /// @brief Finds the "least squares" solution for the equation \f$ \mathbf{v} = \mathbf{dz} - \mathbf{H} \mathbf{x} \f$ | ||
| 34 | /// | ||
| 35 | /// Minimizes the functional | ||
| 36 | /// \anchor eq-LinearLeastSquares-functional \f{equation}{ \label{eq:eq-LinearLeastSquares-functional} | ||
| 37 | /// J(\mathbf{x}) \equiv \sum_{i=1}^m v_i^2 = \mathbf{v}^T \mathbf{v} = (\mathbf{dz} - \mathbf{H} \mathbf{x})^T (\mathbf{dz} - \mathbf{H} \mathbf{x}) | ||
| 38 | /// \f} | ||
| 39 | /// which has the solution (assuming that the inverse to \f$ \mathbf{H}^T \mathbf{H} \f$ exists) | ||
| 40 | /// \anchor eq-LinearLeastSquares-solution \f{equation}{ \label{eq:eq-LinearLeastSquares-solution} | ||
| 41 | /// \mathbf{x} = \left(\mathbf{H}^T \mathbf{H} \right)^{-1} \mathbf{H}^T \mathbf{dz} | ||
| 42 | /// \f} | ||
| 43 | /// @param[in] H Design Matrix | ||
| 44 | /// @param[in] dz Residual vector | ||
| 45 | /// @return Least squares solution | ||
| 46 | template<typename DerivedA, typename DerivedB> | ||
| 47 | Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> solveLinearLeastSquares(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedB>& dz) | ||
| 48 | { | ||
| 49 | return (H.transpose() * H).inverse() * H.transpose() * dz; | ||
| 50 | } | ||
| 51 | |||
| 52 | /// @brief Finds the "weighted least squares" solution | ||
| 53 | /// | ||
| 54 | /// \anchor eq-WeightedLinearLeastSquares \f{equation}{ \label{eq:eq-WeightedLinearLeastSquares} | ||
| 55 | /// \mathbf{x} = \left(\mathbf{H}^T \mathbf{W} \mathbf{H} \right)^{-1} \mathbf{H}^T \mathbf{W} \mathbf{dz} | ||
| 56 | /// \f} | ||
| 57 | /// @param[in] H Design Matrix | ||
| 58 | /// @param[in] W Weight matrix | ||
| 59 | /// @param[in] dz Residual vector | ||
| 60 | /// @return Least squares solution | ||
| 61 | template<typename DerivedA, typename DerivedW, typename DerivedB> | ||
| 62 | Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> solveWeightedLinearLeastSquares(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedW>& W, const Eigen::MatrixBase<DerivedB>& dz) | ||
| 63 | { | ||
| 64 | return (H.transpose() * W * H).inverse() * H.transpose() * W * dz; | ||
| 65 | } | ||
| 66 | |||
| 67 | /// @brief Finds the "least squares" solution for the equation \f$ \mathbf{v} = \mathbf{dz} - \mathbf{H} \mathbf{x} \f$ | ||
| 68 | /// @param[in] H Design Matrix | ||
| 69 | /// @param[in] dz Residual vector | ||
| 70 | /// @return Least squares solution and variance | ||
| 71 | template<typename DerivedA, typename DerivedB> | ||
| 72 | LeastSquaresResult<Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime>> | ||
| 73 | solveLinearLeastSquaresUncertainties(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedB>& dz) | ||
| 74 | { | ||
| 75 | // Cofactor matrix | ||
| 76 | Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime> Q = (H.transpose() * H).inverse(); | ||
| 77 | LOG_DATA("Q = \n{}", Q); | ||
| 78 | // Least squares solution | ||
| 79 | Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> dx = Q * H.transpose() * dz; | ||
| 80 | LOG_DATA("dx = {}", dx.transpose()); | ||
| 81 | |||
| 82 | // Residual sum of squares | ||
| 83 | double RSS = std::pow(dz.norm(), 2); | ||
| 84 | LOG_DATA("RSS = {}", RSS); | ||
| 85 | |||
| 86 | // Amount of equations | ||
| 87 | auto m = H.rows(); | ||
| 88 | // Amount of variables | ||
| 89 | auto n = H.cols(); | ||
| 90 | // Statistical degrees of freedom | ||
| 91 | auto dof = m - n; | ||
| 92 | LOG_DATA("dof = {}", dof); | ||
| 93 | |||
| 94 | // Estimated error variance (reduced chi-squared statistic) | ||
| 95 | double sigma2 = RSS / static_cast<double>(dof); | ||
| 96 | LOG_DATA("sigma2 = {}", sigma2); | ||
| 97 | |||
| 98 | // Covariance matrix | ||
| 99 | Q *= sigma2; | ||
| 100 | LOG_DATA("variance = \n{}", Q); | ||
| 101 | |||
| 102 | return { .solution = dx, .variance = Q }; | ||
| 103 | } | ||
| 104 | |||
| 105 | /// @brief Finds the "weighted least squares" solution | ||
| 106 | /// @param[in] H Design Matrix | ||
| 107 | /// @param[in] W Weight matrix | ||
| 108 | /// @param[in] dz Residual vector | ||
| 109 | /// @return Weighted least squares solution and variance | ||
| 110 | template<typename DerivedA, typename DerivedW, typename DerivedB> | ||
| 111 | LeastSquaresResult<Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime>> | ||
| 112 | ✗ | solveWeightedLinearLeastSquaresUncertainties(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedW>& W, const Eigen::MatrixBase<DerivedB>& dz) | |
| 113 | { | ||
| 114 | // Cofactor matrix | ||
| 115 | ✗ | Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime> Q = (H.transpose() * W * H).inverse(); | |
| 116 | LOG_DATA("Cofactor matrix = \n{}", Q); | ||
| 117 | // Least squares solution | ||
| 118 | ✗ | Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> dx = Q * H.transpose() * W * dz; | |
| 119 | LOG_DATA("dx = {}", dx.transpose()); | ||
| 120 | |||
| 121 | // Residual sum of squares | ||
| 122 | ✗ | double RSS = dz.transpose() * W * dz; | |
| 123 | LOG_DATA("RSS = {}", RSS); | ||
| 124 | |||
| 125 | // Amount of equations | ||
| 126 | ✗ | auto m = H.rows(); | |
| 127 | // Amount of variables | ||
| 128 | ✗ | auto n = H.cols(); | |
| 129 | // Statistical degrees of freedom | ||
| 130 | ✗ | auto dof = m - n; | |
| 131 | LOG_DATA("dof = {}", dof); | ||
| 132 | |||
| 133 | // Estimated error variance (reduced chi-squared statistic) | ||
| 134 | ✗ | double sigma2 = RSS / static_cast<double>(dof); | |
| 135 | LOG_DATA("sigma2 = {}", sigma2); | ||
| 136 | |||
| 137 | // Covariance matrix | ||
| 138 | ✗ | Q *= sigma2; | |
| 139 | LOG_DATA("Covariance matrix = \n{}", Q); | ||
| 140 | |||
| 141 | ✗ | return { .solution = dx, .variance = Q }; | |
| 142 | ✗ | } | |
| 143 | |||
| 144 | } // namespace NAV | ||
| 145 |