// are two Namelists identical, ignoring a permutation?
bool operator==(const Namelist& N1, const Namelist& N2)
{
try {
if(N1.size() != N2.size()) return false;
if(N1.size() == 0) return true;
for(unsigned int i=0; i<N1.size(); i++) {
unsigned int match=0;
for(unsigned int j=0; j<N2.size(); j++)
if(N1.labels[i] == N2.labels[j]) match++;
if(match != 1) return false; // if > 1, N2 is invalid
}
return true;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// --------------------------------------------------------------------------------
// explicit constructor - throw if the dimensions are inconsistent.
SRI::SRI(const Matrix<double>& r,
const Vector<double>& z,
const Namelist& nl)
throw(MatrixException)
{
if(r.rows() != r.cols() || r.rows() != z.size() || r.rows() != nl.size()) {
MatrixException me("Invalid dimensions in explicit SRI constructor:\n R is "
+ asString<int>(r.rows()) + "x"
+ asString<int>(r.cols()) + ", Z has length "
+ asString<int>(z.size()) + " and NL has length "
+ asString<int>(nl.size())
);
GPSTK_THROW(me);
}
if(r.rows() <= 0) return;
R = r;
Z = z;
names = nl;
}
//------------------------------------------------------------------------------------
// explicit constructor - throw if the dimensions are inconsistent.
SRIleastSquares::SRIleastSquares(const Matrix<double>& Rin,
const Vector<double>& Zin,
const Namelist& NLin)
throw(MatrixException)
{
defaults();
if(Rin.rows() != Rin.cols() ||
Rin.rows() != Zin.size() ||
Rin.rows() != NLin.size()) {
MatrixException me("Invalid input dimensions: R is "
+ asString<int>(Rin.rows()) + "x"
+ asString<int>(Rin.cols()) + ", Z has length "
+ asString<int>(Zin.size()) + ", and NL has length "
+ asString<int>(NLin.size())
);
GPSTK_THROW(me);
}
R = Rin;
Z = Zin;
names = NLin;
}
// --------------------------------------------------------------------------------
// Split this SRI (call it S) into two others, S1 and Sleft, where S1 has
// a Namelist identical to the input Namelist (NL); set *this = S1 at the
// end. NL must be a non-empty subset of names, and (names ^ NL) also must
// be non-empty; throw MatrixException if this is not true. The second
// output SRI, Sleft, will have the same names as S, but perhaps permuted.
//
// The routine works by first permuting S so that its Namelist if of the
// form {N2,NL}, where N2 = (names ^ NL); this is possible only if NL is
// a non-trivial subset of names. Then, the rows of S (rows of R and elements
// of Z) naturally separate into the two component SRIs, with zeros in the
// elements of the first SRI which correspond to N2, and those in Sleft
// which correspond to NL.
//
// Example: S.name = A B C D E F G and NL = D E F G.
// (Obviously, S may be permuted into such an order whenever this is needed.)
// Note that here the R,Z pair is written in a format reminiscent of the
// set of equations implied by R*X=Z, i.e. 1A+2B+3C+4D+5E+6F+7G=a, etc.
//
// S (R Z) = S1 + Sleft
// with names NL names
// A B C D E F G . . . D E F G A B C D E F G
// - - - - - - - - - - - - - - - - - - - - - - - -
// 1 2 3 4 5 6 7 a = . . . . . . . . + 1 2 3 4 5 6 7 a
// 8 9 1 2 3 4 b . . . . . . . 8 9 1 2 3 4 b
// 5 6 7 8 9 c . . . . . . 5 6 7 8 9 c
// 1 2 3 4 d 1 2 3 4 d . . . . d
// 5 6 7 e 5 6 7 e . . . e
// 8 9 f 8 9 f . . f
// 1 g 1 g . g
//
// where "." denotes a zero. The split is simply separating the linear
// equations which make up R*X=Z into two groups; because of the ordering,
// one of the groups of equations (S1) depends only on a particular subset
// of the elements of the state vector, i.e. the elements labelled by the
// Namelist NL.
//
// The equation shown here is an information equation; if the two SRIs S1
// and Sleft were merged again, none of the information would be lost.
// Note that S1 has no dependence on A B C (hence the .'s), and therefore
// its size can be reduced. However S2 still depends on the full names
// Namelist. Sleft is necessarily singular, but S1 is not.
//
// Note that the SRI contains information about both the solution and
// the covariance, i.e. state and noise, and therefore one must be very careful
// in interpreting the results of split and merge (operator+=). [Be especially
// careful about the idea that a merge might be reversible with a split() or
// vice-versa - strictly this is never possible unless the Namelists are
// mutually exclusive - two separate problems.]
//
// For example, suppose two different SRI's, which have some elements in common,
// are merged. The combined SRI will have more information (it can't have less)
// about the common elements, and therefore the solution will be 'better'
// (assuming the underlying model equations for those elements are identical).
// However the noises will also be combined, and the results you get might be
// surprising. Also, note that if you then split the combined SRI again, the
// solution won't change but the noises will be very different; in particular
// the new split part will take all the information with it, so the common states
// will have lower noise than they did in the original SRI.
// See the test program tsri.cpp
//
void SRI::split(const Namelist& NL, SRI& Sleft)
throw(MatrixException,VectorException)
{
try {
Sleft = SRI(0);
unsigned int n,m;
n = NL.size();
m = names.size();
if(n >= m) {
MatrixException me("split: Input Namelist must be a subset of this one");
GPSTK_THROW(me);
}
unsigned int i,j;
// copy names and permute it so that its end matches NL
Namelist N0(names);
for(i=1; i<=n; i++) { // loop (backwards) over names in NL
for(j=1; j<=m; j++) { // search (backwards) in NO for a match
if(NL.labels[n-i] == N0.labels[m-j]) { // if found a match
N0.swap(m-i,m-j); // then move matching name to end
break; // and go on to next name in NL
}
}
if(j > m) {
MatrixException me("split: Input Namelist is not non-trivial subset");
GPSTK_THROW(me);
}
}
// copy *this into Sleft, then do the permutation
Sleft = *this;
Sleft.permute(N0);
// copy parts of Sleft into S1, and then zero out those parts of Sleft
SRI S1(NL);
S1.R = Matrix<double>(Sleft.R,m-n,m-n,n,n);
//S1.Z = Vector<double>(Sleft.Z,m-n,n);
S1.Z.resize(n);
for(i=0; i<n; i++) S1.Z(i) = Sleft.Z(m-n+i);
for(i=m-n; i<m; i++) Sleft.zeroOne(i);
*this = S1;
}
catch(MatrixException& me) {
GPSTK_RETHROW(me);
}
catch(VectorException& ve) {
GPSTK_RETHROW(ve);
}
}
// ---------------------------------------------------------------------------
// modify SRIs
// --------------------------------------------------------------------------------
// Permute the SRI elements to match the input Namelist, which may differ with
// the SRI Namelist by AT MOST A PERMUTATION, throw if this is not true.
void SRI::permute(const Namelist& nl)
throw(MatrixException,VectorException)
{
if(identical(names,nl)) return;
if(names != nl) {
MatrixException me("Invalid input: Namelists must be == to permute");
GPSTK_THROW(me);
}
try {
unsigned int i,j;
// build a permutation matrix
Matrix<double> P(R.rows(),R.rows(),0.0);
for(i=0; i<R.rows(); i++) {
j = nl.index(names.getName(i));
P(j,i) = 1;
}
Matrix<double> B;
Vector<double> Q;
B = P * R * transpose(P);
Q = P * Z;
// re-triangularize
R = 0.0;
Z = 0.0;
SrifMU(R,Z,B,Q);
names = nl;
}
catch(MatrixException& me) {
GPSTK_RETHROW(me);
}
catch(VectorException& ve) {
GPSTK_RETHROW(ve);
}
}
//------------------------------------------------------------------------------------
// called by Estimation() - inside the data loop, inside the iteration loop
// Input is Namelist DNL, the double difference data Namelist (DataNL)
// Output is MCov, the measurement covariance matrix for this data (MeasCov).
// Let:
// d = vector of one-way data (one site, one satellite)
// sd = vector of single difference data (two sites, one satellite)
// dd = vector of double difference data (two sites, two satellites)
// DD and SD are matricies with elements 0,1,-1 which transform d to sd to dd:
// sd = SD * d
// dd = DD * sd
// dd = DD * SD * d
// The covariance matrix will be MC = (DD*SD)*transpose(DD*SD)
// = DD*SD*transpose(SD)*transpose(DD)
// If the one-way data has a measurement covariance, then fill the vector d with
// them; then MC = DD*SD* d * transpose(SD)*transpose(DD).
// Building DD and SD is just a matter of lists:
// loop through the dd namelist, keeping lists of:
// one-way data (site-satellite pairs) (d)
// single differences (site-site-satellite sets) (sd)
// and you have a list of double differences (DNL)
//
void BuildStochasticModel(int count, Namelist& DNL, Matrix<double>& MCov)
throw(Exception)
{
try {
unsigned int m=DNL.size();
if(m==0) return;
int i,j,in,jn,kn;
string site1,site2;
GSatID sat1,sat2;
vector<double> d; // weights of one-way data
vector<OWid> ld; // labels of d
vector<SDid> sd;
for(i=0; i<DNL.size(); i++) {
// break the label into site1,site2,sat1,sat2
DecomposeName(DNL.getName(i), site1, site2, sat1, sat2);
if(index(ld,OWid(site1,sat1)) == -1) ld.push_back(OWid(site1,sat1));
if(index(ld,OWid(site1,sat2)) == -1) ld.push_back(OWid(site1,sat2));
if(index(ld,OWid(site2,sat1)) == -1) ld.push_back(OWid(site2,sat1));
if(index(ld,OWid(site2,sat2)) == -1) ld.push_back(OWid(site2,sat2));
if(index(sd,SDid(site1,site2,sat1)) == -1) sd.push_back(SDid(site1,site2,sat1));
if(index(sd,SDid(site1,site2,sat2)) == -1) sd.push_back(SDid(site1,site2,sat2));
}
// fill d with the weights
d = vector<double>(ld.size());
for(i=0; i<ld.size(); i++) d[i] = StochasticWeight(ld[i], count);
// temp
//format f113s(11,3,2);
//oflog << "DDs are (" << DNL.size() << "):\n" << setw(20) << DNL << endl;
//oflog << "SDs are: (" << sd.size() << ")" << fixed << endl;
//for(i=0; i<sd.size(); i++) oflog << " / " << sd[i];
//oflog << endl;
//oflog << "OWs are: (" << ld.size() << ")" << endl;
//for(i=0; i<ld.size(); i++) oflog << " / " << ld[i];
//oflog << endl;
//oflog << "OW wts are: (" << d.size() << ")" << endl;
//for(i=0; i<d.size(); i++) oflog << " " << f113s << d[i];
//oflog << endl;
Matrix<double> SD(sd.size(),ld.size(),0.0);
Matrix<double> DD(m,sd.size(),0.0);
// TD need to account for signs here ... sd[.] may be site2,site1,sat1 ...
for(in=0; in<DNL.size(); in++) {
DecomposeName(DNL.getName(in), site1, site2, sat1, sat2);
jn = index(sd,SDid(site1,site2,sat1)); // site1-site2, sat1
DD(in,jn) = 1;
kn = index(ld,OWid(site1,sat1)); // site1, sat1
SD(jn,kn) = d[kn];
kn = index(ld,OWid(site2,sat1)); // site2, sat1
SD(jn,kn) = -d[kn];
jn = index(sd,SDid(site1,site2,sat2)); // site1-site2, sat2
DD(in,jn) = -1;
kn = index(ld,OWid(site1,sat2)); // site1, sat2
SD(jn,kn) = d[kn];
kn = index(ld,OWid(site2,sat2)); // site2, sat2
SD(jn,kn) = -d[kn];
}
//oflog << " SD is\n" << fixed << setw(3) << SD << endl;
//oflog << " DD is\n" << fixed << setw(3) << DD << endl;
Matrix<double> T;
T = DD * SD;
MCov = T * transpose(T);
static bool once=true;
if(once) {
oflog << "Measurement covariance (model " << CI.StochasticModel << ") is\n"
<< scientific << setw(8) << setprecision(3) << MCov << endl;
once = false;
}
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}
// --------------------------------------------------------------------------------
// Vector version of stateFix with several states given in a Namelist.
void SRI::stateFix(const Namelist& dropNL, const Vector<double>& values_in)
throw(MatrixException,VectorException)
{
try {
if(dropNL.size() != values_in.size()) {
VectorException e("Input has inconsistent lengths");
GPSTK_THROW(e);
}
/*
// build a vector of indexes to keep
int i,j;
vector<int> indexes;
for(i=0; i<names.size(); i++) {
j = dropNL.index(names.getName(i)); // index in dropNL, this state
if(j == -1) indexes.push_back(i);// not found in dropNL, so keep
}
const int n=indexes.size(); // new dimension
if(n == 0) {
Exception e("Cannot drop all states");
GPSTK_THROW(e);
}
Vector<double> X,newX(n);
Matrix<double> C,newC(n,n);
Namelist newNL;
double big,small;
getStateAndCovariance(X,C,&small,&big);
for(i=0; i<n; i++) {
newX(i) = X(indexes[i]);
for(j=0; j<n; j++) newC(i,j) = C(indexes[i],indexes[j]);
newNL += names.getName(indexes[i]);
}
R = Matrix<double>(inverseUT(upperCholesky(newC)));
Z = Vector<double>(R*newX);
names = newNL;
*/
size_t i,j,k;
// create a vector of indexes and corresponding values
vector<int> indx;
vector<double> value;
for(i=0; i<dropNL.size(); i++) {
int in = names.index(dropNL.getName(i)); // in must be allowed to be -1
if(in > -1) {
indx.push_back(in);
value.push_back(values_in(i));
}
//else nothing happens
}
const unsigned int m = indx.size();
const unsigned int n = R.rows();
if(m == 0) return;
if(m == n) {
*this = SRI(0);
return;
}
// move the X(in) terms to the data vector on the RHS
for(k=0; k<m; k++)
for(i=0; i<indx[k]; i++)
Z(i) -= R(i,indx[k])*value[k];
// first remove the rows in indx
bool skip;
Vector<double> Ztmp(n-m,0.0);
Matrix<double> Rtmp(n-m,n,0.0);
for(i=0,k=0; i<n; i++) {
skip = false;
for(j=0; j<m; j++) if((int)i == indx[j]) { skip=true; break; }
if(skip) continue; // skip row to be dropped
Ztmp(k) = Z(i);
for(j=i; j<n; j++) Rtmp(k,j) = R(i,j);
k++;
}
// Z is now done
Z = Ztmp;
// now remove columns in indx
R = Matrix<double>(n-m,n-m,0.0);
for(j=0,k=0; j<n; j++) {
skip = false;
for(i=0; i<m; i++) if((int)j == indx[i]) { skip=true; break; }
if(skip) continue; // skip col to be dropped
for(i=0; i<=j; i++) R(i,k) = Rtmp(i,j);
k++;
}
// remove the names
for(k=0; k<dropNL.size(); k++) {
std::string name(dropNL.getName(k));
names -= name;
}
}
catch(MatrixException& me) {
GPSTK_RETHROW(me);
}
catch(VectorException& ve) {
GPSTK_RETHROW(ve);
}
}
//------------------------------------------------------------------------------------
// Read all the files on the command line, they should contain covariance and state
// with labels. Merge all these SRIs and output the final covariance and state.
int main(int argc, char **argv)
{
try {
bool verbose=false;
int i,n,N,nfile,nline,nword;
string line,word;
Matrix<double> cov;
Vector<double> state;
Namelist name;
SRI S;
if(argc <= 1) {
cout << "Prgm mergeSRI combines solution and covariance results from "
<< "different sources\n into a single result. Each file named on the "
<< "command line consists of lines,\n one per row of the covariance "
<< "matrix, of the form\n label(i) cov(i,0) cov(i,1) ... cov(i,n) "
<< "solution(i)\n where there are n lines in the file (i.e. the "
<< "covariance matrix is square)\n and labels are used consistently "
<< "among all the results in all the files.\n Results are output as "
<< "a single combined namelist, covariance and solution.\n";
return 0;
}
nfile = 0;
for(i=1; i<argc; i++) {
if(string(argv[i]) == string("-v") ||
string(argv[i]) == string("--verbose")) {
verbose = true;
continue;
}
ifstream ifs(argv[i]);
if(!ifs) {
cout << "Could not open file " << argv[i] << endl;
continue;
}
if(verbose) cout << "Opened file " << argv[i] << endl;
// read the file
N = nline = 0; // N is the dimension of cov and state and name
while(!ifs.eof() && ifs.good()) {
getline(ifs,line);
StringUtils::stripTrailing(line,'\r');
if(ifs.bad()) break;
StringUtils::stripLeading(line);
if(line.empty()) break;
n = StringUtils::numWords(line);
if(N == 0) {
N = n-2;
cov = Matrix<double>(N,N,0.0);
state = Vector<double>(N,0.0);
name.clear();
}
else if(n-2 != N) {
cerr << "Warning - dimensions are wrong in file " << argv[i]
<< " : " << n-2 << " != " << N << endl;
}
nword = 0;
while(1) {
word = StringUtils::stripFirstWord(line);
if(word.empty()) break;
if(nword == 0) {
name += word;
}
else if(nword < N+1) {
cov(nline,nword-1) = StringUtils::asDouble(word);
}
else if(nword == N+1) {
state(nline) = StringUtils::asDouble(word);
}
nword++;
};
nline++;
if(nline > N) break;
}
ifs.close();
if(N <= 0 || name.size() <= 0) {
cout << "Empty file - ignore : " << argv[i] << endl;
continue;
}
name.resize(N);
cout << "Add file " << argv[i] << " : state names " << name << endl;
if(verbose) {
LabeledVector Lstate(name,state);
Lstate.fixed().setw(16).setprecision(6);
cout << "State" << endl << Lstate << endl;
LabeledMatrix Lcov(name,cov);
Lcov.scientific().setw(16).setprecision(6);
cout << "Covariance" << endl << Lcov << endl;
}
SRI S1(name);
S1.addAPriori(cov,state);
S += S1;
nfile++;
}
if(nfile <= 0) {
cout << "No files!\n";
return 0;
}
double small,big;
S.getStateAndCovariance(state,cov,&small,&big);
cout << endl;
LabeledVector Ls(name,state);
Ls.fixed().setw(16).setprecision(6);
cout << "Final state" << endl << Ls << endl;
LabeledMatrix Lc(name,cov);
Lc.scientific().setw(16).setprecision(6);
cout << endl << "Final covariance" << endl << Lc << endl;
}
catch(MatrixException& me) {
cerr << "Exception: " << me << endl;
return -1;
}
return 0;
}
