GaussHelmert.cpp

#include "GaussHelmert.hh"

#include <Base/Math/Vector.hh>
#include <Base/Math/Matrix.hh>
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;


BIAS::GaussHelmert::
GaussHelmert()
  : Debug(), MaxNumIter_(40), Precision_(1e-8)
{}

BIAS::GaussHelmert::
~GaussHelmert()
{}

int BIAS::GaussHelmert::
Solve(Vector<double>& observations, const Matrix<double>& obs_cov, 
      Vector<double>& x, Matrix<double>& cov)
{
  // the number of observations
  const int N = (int)observations.size();
  
  double sigma_2_hat = 1.0;
  Vector<double> x_hat = x;
  Vector<double> l_hat = observations;
  Vector<double> v_hat(N);
  v_hat.SetZero();
  Matrix<double> Sigma_ll = obs_cov;
  SVD svd;
  Matrix<double> Sigma_ll_plus = svd.Invert(Sigma_ll);
  Matrix<double> A, B, H;
  A = GetA_(l_hat, x_hat);
  B = GetB_(l_hat, x_hat);
  H = GetH_(x_hat);

  // number of constraints on observations and unknowns
  const int G = A.num_rows(); 
  // number of unknowns
  const int U = A.num_cols();
  // number of constraints on unknowns
  const int Hn = H.num_cols();

  const int R = G + Hn - U;
  
  Matrix<double> Weight, WeightInverse, ATWeightInverse, ATWeightInverseA;
  Matrix<double> HT, normal_matrix(Hn+U, Hn+U);
  Vector<double> w_g, w_h, ATWeightInverse_w_g, normal_vector(Hn+U), x_mu;
  Vector<double> delta_x_hat(U), mu(Hn), lambda;

  int res = 0;
  int num_iter = 0;
  do {

    A = GetA_(l_hat, x_hat);
    B = GetB_(l_hat, x_hat);
    H = GetH_(x_hat);

    ComputeNormalSystem_(A, B, H, Sigma_ll, x_hat, l_hat, v_hat,
                         normal_matrix, normal_vector);

    // solve the system and get x_hat and mu
    if ((res = svd.Solve(normal_matrix, normal_vector, x_mu))!=0){
      BIASERR("error solving normal equation "<<res);
      return -1;
    }
  
    for (int r=0; r<U; r++){
      delta_x_hat[r] = x_mu[r];
    }
    for (int r=U; r<U+Hn; r++){
      mu[r-U] = x_mu[r];
    }

    // compute lambda
    lambda = WeightInverse * ( A * delta_x_hat - w_g );

    // compute v
    v_hat = -1.0 * Sigma_ll * B * lambda;

    // update x_hat and l_hat
    x_hat += delta_x_hat;
    l_hat += v_hat;
    
    sigma_2_hat = v_hat.ScalarProduct(Sigma_ll_plus * v_hat) / (double)R;

    Sigma_ll += sigma_2_hat * Sigma_ll;
    Sigma_ll_plus = svd.Invert(Sigma_ll);

    num_iter++;

  } while (num_iter<MaxNumIter_ && sigma_2_hat>Precision_);

  // compute the covariance matrix
  ComputeNormalSystem_(A, B, H, Sigma_ll, x_hat, l_hat, v_hat,
                       normal_matrix, normal_vector);
  Matrix<double> tmp_cov = svd.Invert(normal_matrix);
  for (int r=0; r<U; r++){
    for (int c=0; c<U; c++){
      cov[r][c] = tmp_cov[r][c] * sigma_2_hat;
    }
  }
  x = x_hat;
  observations = l_hat;
  
  return 0;
}


void BIAS::GaussHelmert::
ComputeNormalSystem_(const Matrix<double>& A, const Matrix<double>& B,
                     const Matrix<double>& H, const Matrix<double>& Sigma_ll,
                     const Vector<double>& x_hat, const Vector<double>& l_hat,
                     const Vector<double>& v_hat,
                     Matrix<double>& normal_matrix, Vector<double>& normal_vec)
{
  Matrix<double> Weight, WeightInverse, ATWeightInverse, ATWeightInverseA, HT;
  Vector<double> w_g, w_h, ATWeightInverse_w_g;
  SVD svd;

  // number of unknowns
  const int U = A.num_cols();
  // number of constraints on unknowns
  const int Hn = H.num_cols();

  Weight = B.Transpose() * Sigma_ll * B;
  WeightInverse = svd.Invert(Weight);
  
  ATWeightInverse = A.Transpose() * WeightInverse;
  ATWeightInverseA = ATWeightInverse * A;
  HT = H.Transpose();
  
  w_g = -1.0 * g_(l_hat, x_hat) - B.Transpose() * v_hat;
  w_h = -1.0 * h_(x_hat);
  ATWeightInverse_w_g = ATWeightInverse * w_g;
  
  normal_matrix.SetZero();

  // fill the normal vector
  for (int r=0; r<U; r++){
    normal_vec[r] = ATWeightInverse_w_g[r];
  }
  for (int r=U; r<U+Hn; r++){
    normal_vec[r] = w_h[r-U];
  }

  // fill the normal matrix
  for (int r=0; r<U; r++){
    for (int c=0; c<U; c++){
      normal_matrix[r][c] = ATWeightInverseA[r][c];
    }
  }
  for (int r=0; r<U; r++){ // H
    for (int c=U; c<U+Hn; c++){
      normal_matrix[r][c] = H[r][c-U];
    }
  }
  for (int r=U; r<U+Hn; r++){ // HT
    for (int c=0; c<U; c++){
      normal_matrix[r][c] = HT[r-U][c];
    }
  }
}