LeastSquares.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <cmath>
#include "util/Logger.hpp"
 
namespace NAV
{
 
template<typename SolutionVector, typename VarianceMatrix>
struct LeastSquaresResult
{
    SolutionVector solution; 
    VarianceMatrix variance; 
};
 
template<typename DerivedA, typename DerivedB>
Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> solveLinearLeastSquares(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedB>& dz)
{
    return (H.transpose() * H).inverse() * H.transpose() * dz;
}
 
template<typename DerivedA, typename DerivedW, typename DerivedB>
Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> solveWeightedLinearLeastSquares(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedW>& W, const Eigen::MatrixBase<DerivedB>& dz)
{
    return (H.transpose() * W * H).inverse() * H.transpose() * W * dz;
}
 
template<typename DerivedA, typename DerivedB>
LeastSquaresResult<Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime>>
    solveLinearLeastSquaresUncertainties(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedB>& dz)
{
    // Cofactor matrix
    Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime> Q = (H.transpose() * H).inverse();
    LOG_DATA("Q = \n{}", Q);
    // Least squares solution
    Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> dx = Q * H.transpose() * dz;
    LOG_DATA("dx = {}", dx.transpose());
 
    // Residual sum of squares
    double RSS = std::pow(dz.norm(), 2);
    LOG_DATA("RSS = {}", RSS);
 
    // Amount of equations
    auto m = H.rows();
    // Amount of variables
    auto n = H.cols();
    // Statistical degrees of freedom
    auto dof = m - n;
    LOG_DATA("dof = {}", dof);
 
    // Estimated error variance (reduced chi-squared statistic)
    double sigma2 = RSS / static_cast<double>(dof);
    LOG_DATA("sigma2 = {}", sigma2);
 
    // Covariance matrix
    Q *= sigma2;
    LOG_DATA("variance = \n{}", Q);
 
    return { .solution = dx, .variance = Q };
}
 
template<typename DerivedA, typename DerivedW, typename DerivedB>
LeastSquaresResult<Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime>>
    solveWeightedLinearLeastSquaresUncertainties(const Eigen::MatrixBase<DerivedA>& H, const Eigen::MatrixBase<DerivedW>& W, const Eigen::MatrixBase<DerivedB>& dz)
{
    // Cofactor matrix
    Eigen::Matrix<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime, DerivedA::ColsAtCompileTime> Q = (H.transpose() * W * H).inverse();
    LOG_DATA("Cofactor matrix = \n{}", Q);
    // Least squares solution
    Eigen::Vector<typename DerivedA::Scalar, DerivedA::ColsAtCompileTime> dx = Q * H.transpose() * W * dz;
    LOG_DATA("dx = {}", dx.transpose());
 
    // Residual sum of squares
    double RSS = dz.transpose() * W * dz;
    LOG_DATA("RSS = {}", RSS);
 
    // Amount of equations
    auto m = H.rows();
    // Amount of variables
    auto n = H.cols();
    // Statistical degrees of freedom
    auto dof = m - n;
    LOG_DATA("dof = {}", dof);
 
    // Estimated error variance (reduced chi-squared statistic)
    double sigma2 = RSS / static_cast<double>(dof);
    LOG_DATA("sigma2 = {}", sigma2);
 
    // Covariance matrix
    Q *= sigma2;
    LOG_DATA("Covariance matrix = \n{}", Q);
 
    return { .solution = dx, .variance = Q };
}
 
} // namespace NAV