%                                qrupa.m
%  Scope:   This MATLAB macro implements the updating algorithm [1] for Q-R 
%           factorization of the modified measurement matrix when a new 
%           clock measurement is added. 
%  Usage:   [qupdate,rupdate] = qrupa(m,n,q,r,alpha)
%  Description of parameters:
%           m       - input, real scalar, number of rows and columns in matrix
%                     q, and number of rows in matrix  r
%           n       - input, real scalar, number of columns in matrix  r
%           q       - input, real two-dimensional array storing the initial
%                     matrix q, where measurement matrix is decomposed in (q,r)
%                     factors
%           r       - input, real two-dimensional array storing the initial
%                     matrix r, where measurement matrix is decomposed in (q,r)
%                     factors
%           alpha   - input, real scalar, ratio between pseudorange measurement 
%                     standard deviation and clock standard deviation
%           qupdate - output, real two-dimensional array storing the updated q
%                     matrix  qupdate
%           rupdate - output, real two-dimensional array storing the updated r
%                     matrix  rupdate
%  Reference:
%          [1] Copps, E. M., Lupash, L., Extending the generality and
%              robustness of RAIM algorithms. Proceedings of the 1994 National
%              Technical Meeting, Institute of Navigation, San Diego, CA, Jan. 
%              24-26, 1994, pp. 31-39.
%  Last update:  07/31/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [qupdate,rupdate] = qrupa(m,n,q,r,alpha)

mp1 = m + 1;
rnn = r(n,n);
temp = sqrt(rnn * rnn + alpha * alpha);
cnmp1 = rnn / temp;
snmp1 = alpha / temp;

zeron = zeros(n,1);
zeromp1 = zeros(mp1,1);
rupdate = [r' zeron]';
rupdate(n,n) = temp;

qupdate = [q zeros(m,1)];
qupdate = [qupdate' zeros(mp1,1)]';
for i = 1:m
   qupdate(i,n) = q(i,n) * cnmp1;
   qupdate(i,mp1) = - q(i,n) * snmp1;
end
qupdate(mp1,n) =  snmp1;
qupdate(mp1,mp1) = cnmp1;