function [v] = RegrReconc(v,R,G,g)

%   [v] = RegrReconc(v,R,G,g)
%
% Data reconciliation. 
%
% Input parameters:
%  - v: Original data vector (n x 1)
%  - R: Assumed measurement covariance matrix (n x n)
%  - G,g: Constraints so that there should hold Gv = g
% Return parameters:
%  - v: Modified data vector (n x 1)
%
% Heikki Hyotyniemi Jan.1, 2003


v = NaN;
if size(v,1) ~= sqrt(size(R,1)*size(R,2)), disp('Incompatible v and R'); break; end
if size(v,1) ~= size(G,2), disp('Incompatible v and G'); break; end
if size(g,1) ~= size(G,1), disp('Incompatible g and G'); break; end

v = v - R*G'*inv(G*R*G')*(G*v-g);