0.4.1
Loading...
Searching...
No Matches
LeastSquares.hpp
Go to the documentation of this file.
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
22namespace NAV
23{
24
25/// @brief Least Squares Uncertainties return value
26template<typename SolutionVector, typename VarianceMatrix>
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
46template<typename DerivedA, typename DerivedB>
47Eigen::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
61template<typename DerivedA, typename DerivedW, typename DerivedB>
62Eigen::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
71template<typename DerivedA, typename DerivedB>
72LeastSquaresResult<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
110template<typename DerivedA, typename DerivedW, typename DerivedB>
111LeastSquaresResult<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
Utility class for logging to console and file.
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
KeyedLeastSquaresResult< Scalar, StateKeyType > solveLinearLeastSquaresUncertainties(const KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > &H, const KeyedVectorX< Scalar, MeasKeyType > &dz)
Finds the "least squares" solution for the equation .
KeyedVectorX< Scalar, StateKeyType > solveLinearLeastSquares(const KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > &H, const KeyedVectorX< Scalar, MeasKeyType > &dz)
Finds the "least squares" solution for the equation .
KeyedVectorX< Scalar, StateKeyType > solveWeightedLinearLeastSquares(const KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > &H, const KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > &W, const KeyedVectorX< Scalar, MeasKeyType > &dz)
Finds the "weighted least squares" solution (see LeastSquares.hpp)
KeyedLeastSquaresResult< Scalar, StateKeyType > solveWeightedLinearLeastSquaresUncertainties(const KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > &H, const KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > &W, const KeyedVectorX< Scalar, MeasKeyType > &dz)
Finds the "weighted least squares" solution.
Least Squares Uncertainties return value.
VarianceMatrix variance
Least squares variance.
SolutionVector solution
Least squares solution.