SRIFilter.cpp
Go to the documentation of this file.
1 //==============================================================================
2 //
3 // This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
4 //
5 // The GNSSTk is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published
7 // by the Free Software Foundation; either version 3.0 of the License, or
8 // any later version.
9 //
10 // The GNSSTk is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public
16 // License along with GNSSTk; if not, write to the Free Software Foundation,
17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
18 //
19 // This software was developed by Applied Research Laboratories at the
20 // University of Texas at Austin.
21 // Copyright 2004-2022, The Board of Regents of The University of Texas System
22 //
23 //==============================================================================
24 
25 //==============================================================================
26 //
27 // This software was developed by Applied Research Laboratories at the
28 // University of Texas at Austin, under contract to an agency or agencies
29 // within the U.S. Department of Defense. The U.S. Government retains all
30 // rights to use, duplicate, distribute, disclose, or release this software.
31 //
32 // Pursuant to DoD Directive 523024
33 //
34 // DISTRIBUTION STATEMENT A: This software has been approved for public
35 // release, distribution is unlimited.
36 //
37 //==============================================================================
38 
49 #include "SRIFilter.hpp"
50 #include "RobustStats.hpp"
51 #include "StringUtils.hpp"
52 
53 using namespace std;
54 
55 namespace gnsstk
56 {
57 
58  using namespace StringUtils;
59 
60  //---------------------------------------------------------------------------------
61  // empty constructor
62  SRIFilter::SRIFilter() { defaults(); }
63 
64  //---------------------------------------------------------------------------------
65  // constructor given the dimension N.
66  SRIFilter::SRIFilter(const unsigned int N)
67  {
68  defaults();
69  R = Matrix<double>(N, N, 0.0);
70  Z = Vector<double>(N, 0.0);
71  names = Namelist(N);
72  }
73 
74  //---------------------------------------------------------------------------------
75  // constructor given a Namelist, its dimension determines the SRI dimension.
76  SRIFilter::SRIFilter(const Namelist& NL)
77  {
78  defaults();
79  if (NL.size() <= 0)
80  {
81  return;
82  }
83  R = Matrix<double>(NL.size(), NL.size(), 0.0);
84  Z = Vector<double>(NL.size(), 0.0);
85  names = NL;
86  }
87 
88  //---------------------------------------------------------------------------------
89  // explicit constructor - throw if the dimensions are inconsistent.
90  SRIFilter::SRIFilter(const Matrix<double>& Rin, const Vector<double>& Zin,
91  const Namelist& NLin)
92  {
93  defaults();
94  if (Rin.rows() != Rin.cols() || Rin.rows() != Zin.size() ||
95  Rin.rows() != NLin.size())
96  {
97  MatrixException me("Invalid input dimensions: R is " +
98  asString<int>(Rin.rows()) + "x" +
99  asString<int>(Rin.cols()) + ", Z has length " +
100  asString<int>(Zin.size()) + ", and NL has length " +
101  asString<int>(NLin.size()));
102  GNSSTK_THROW(me);
103  }
104  R = Rin;
105  Z = Zin;
106  names = NLin;
107  }
108 
109  //---------------------------------------------------------------------------------
110  // operator=
111  SRIFilter& SRIFilter::operator=(const SRIFilter& right)
112  {
113  R = right.R;
114  Z = right.Z;
115  names = right.names;
116  // valid = right.valid;
117  return *this;
118  }
119 
120  //---------------------------------------------------------------------------------
121  /* SRIF (Kalman) measurement update, or least squares update
122  Returns unwhitened residuals in D */
123  void SRIFilter::measurementUpdate(const Matrix<double>& H, Vector<double>& D,
124  const Matrix<double>& CM)
125  {
126  if (H.cols() != R.cols() || H.rows() != D.size() ||
127  (&CM != &SRINullMatrix &&
128  (CM.rows() != D.size() || CM.cols() != D.size())))
129  {
130  string msg("\nInvalid input dimensions:\n SRI is ");
131  msg += asString<int>(R.rows()) + "x" + asString<int>(R.cols()) +
132  ",\n Partials is " + asString<int>(H.rows()) + "x" +
133  asString<int>(H.cols()) + ",\n Data has length " +
134  asString<int>(D.size());
135  if (&CM != &SRINullMatrix)
136  {
137  msg += ",\n and Cov is " + asString<int>(CM.rows()) + "x" +
138  asString<int>(CM.cols());
139  }
140 
141  MatrixException me(msg);
142  GNSSTK_THROW(me);
143  }
144  try
145  {
146  // whiten partials and data
147  Matrix<double> P(H);
148  Matrix<double> CHL;
149  if (&CM != &SRINullMatrix)
150  {
151  CHL = lowerCholesky(CM);
152  Matrix<double> L(inverseLT(CHL));
153  P = L * P;
154  D = L * D;
155  }
156 
157  // update *this with the whitened information
158  SrifMU(R, Z, P, D);
159 
160  // un-whiten residuals
161  if (&CM != &SRINullMatrix) // same if above creates CHL
162  {
163  D = CHL * D;
164  }
165  }
166  catch (MatrixException& me)
167  {
168  GNSSTK_RETHROW(me);
169  }
170  catch (VectorException& ve)
171  {
172  GNSSTK_RETHROW(ve);
173  }
174  }
175 
176  //---------------------------------------------------------------------------------
177  /* SRIF (Kalman) measurement update, or least squares update -- SparseMatrix
178  version Returns unwhitened residuals in D */
179  void SRIFilter::measurementUpdate(const SparseMatrix<double>& H,
180  Vector<double>& D,
181  const SparseMatrix<double>& CM)
182  {
183  if (H.cols() != R.cols() || H.rows() != D.size() ||
184  (&CM != &SRINullSparseMatrix &&
185  (CM.rows() != D.size() || CM.cols() != D.size())))
186  {
187  string msg("\nInvalid input dimensions:\n SRI is ");
188  msg += asString<int>(R.rows()) + "x" + asString<int>(R.cols()) +
189  ",\n Partials is " + asString<int>(H.rows()) + "x" +
190  asString<int>(H.cols()) + ",\n Data has length " +
191  asString<int>(D.size());
192  if (&CM != &SRINullSparseMatrix)
193  {
194  msg += ",\n and Cov is " + asString<int>(CM.rows()) + "x" +
195  asString<int>(CM.cols());
196  }
197 
198  MatrixException me(msg);
199  GNSSTK_THROW(me);
200  }
201  try
202  {
203  SparseMatrix<double> A(H || D);
204  SparseMatrix<double> CHL;
205  // whiten partials and data
206  if (&CM != &SRINullSparseMatrix)
207  {
208  CHL = lowerCholesky(CM);
209  SparseMatrix<double> L(inverseLT(CHL));
210  A = L * A;
211  }
212 
213  // update *this with the whitened information
214  SrifMU(R, Z, A);
215 
216  // copy out D and un-whiten residuals
217  D = Vector<double>(A.colCopy(A.cols() - 1));
218  if (&CM != &SRINullSparseMatrix)
219  { // same if above creates CHL
220  D = CHL * D;
221  }
222  }
223  catch (MatrixException& me)
224  {
225  GNSSTK_RETHROW(me);
226  }
227  catch (VectorException& ve)
228  {
229  GNSSTK_RETHROW(ve);
230  }
231  }
232 
233  //---------------------------------------------------------------------------------
234  // SRIF (Kalman) time update see SrifTU for doc.
235  void SRIFilter::timeUpdate(Matrix<double>& PhiInv, Matrix<double>& Rw,
236  Matrix<double>& G, Vector<double>& Zw,
237  Matrix<double>& Rwx)
238  {
239  try
240  {
241  SrifTU(R, Z, PhiInv, Rw, G, Zw, Rwx);
242  }
243  catch (MatrixException& me)
244  {
245  GNSSTK_RETHROW(me);
246  }
247  }
248 
249  //---------------------------------------------------------------------------------
250  // SRIF (Kalman) smoother update see SrifSU for doc.
251  void SRIFilter::smootherUpdate(Matrix<double>& Phi, Matrix<double>& Rw,
252  Matrix<double>& G, Vector<double>& Zw,
253  Matrix<double>& Rwx)
254  {
255  try
256  {
257  SrifSU(R, Z, Phi, Rw, G, Zw, Rwx);
258  }
259  catch (MatrixException& me)
260  {
261  GNSSTK_RETHROW(me);
262  }
263  }
264 
265  //---------------------------------------------------------------------------------
266  void SRIFilter::DMsmootherUpdate(Matrix<double>& P, Vector<double>& X,
267  Matrix<double>& Phinv, Matrix<double>& Rw,
268  Matrix<double>& G, Vector<double>& Zw,
269  Matrix<double>& Rwx)
270  {
271  try
272  {
273  SrifSU_DM(P, X, Phinv, Rw, G, Zw, Rwx);
274  }
275  catch (MatrixException& me)
276  {
277  GNSSTK_RETHROW(me);
278  }
279  }
280 
281  //---------------------------------------------------------------------------------
282  // reset the computation, i.e. remove all stored information
283  void SRIFilter::zeroAll() { SRI::zeroAll(); }
284 
285  //---------------------------------------------------------------------------------
286  /* reset the computation, i.e. remove all stored information, and
287  optionally change the dimension. If N is not input, the
288  dimension is not changed.
289  @param N new SRIFilter dimension (optional). */
290  void SRIFilter::reset(const int N)
291  {
292  if (N > 0 && N != (int)R.rows())
293  {
294  R.resize(N, N, 0.0);
295  Z.resize(N, 0.0);
296  }
297  else
298  {
299  SRI::zeroAll(N);
300  }
301  }
302 
303  //---------------------------------------------------------------------------------
304  // private beyond this
305  //---------------------------------------------------------------------------------
306 
307  //---------------------------------------------------------------------------------
308  /* Kalman time update.
309  This routine uses the Householder transformation to propagate the
310  SRIFilter state and covariance through a time step. Input: R a
311  priori square root information (SRI) matrix (an n by n
312  upper triangular matrix)
313  Z a priori SRIF state vector, of length n (state is X, Z = R*X).
314  PhiInv Inverse of state transition matrix, an n by n matrix.
315  PhiInv is destroyed on output.
316  Rw a priori square root information matrix for the process
317  noise, an ns by ns upper triangular matrix
318  G The n by ns matrix associated with process noise. The
319  process noise covariance is G*Q*transpose(G) where inverse(Q)
320  is transpose(Rw)*Rw. G is destroyed on output.
321  Zw a priori 'state' associated with the process noise,
322  a vector with ns elements. Usually set to zero by
323  the calling routine (for unbiased process noise).
324  Rwx An ns by n matrix which is set to zero by this routine
325  but is used for output.
326 
327  Output:
328  The updated square root information matrix and SRIF state (R,Z) and
329  the matrices which are used in smoothing: Rw, Zw, Rwx.
330  Note that PhiInv and G are trashed, and that Rw and Zw are modified.
331 
332  Return values:
333  SrifTU returns void, but throws exceptions if the input matrices
334  or vectors have incompatible dimensions or incorrect types.
335 
336  Method:
337  This SRIF time update method treats the process noise and mapping
338  information as a separate data equation, and applies a Householder
339  transformation to the (appended) equations to solve for an updated
340  state. Thus there is another 'state' variable associated with
341  whatever state variables have process noise. The matrix G relates
342  the process noise variables to the regular state variables, and
343  appears in the term GQG(trans) of the covariance. If all n state
344  variables have process noise, then ns=n and G is an n by n matrix.
345  Since some (or all) of the state variables may not have process
346  noise, ns may be zero. [Bierman ftnt pg 122 seems to indicate that
347  variables with zero process noise can be handled by ns=n & setting a
348  column of G=0. But note that the case of the matrix G=0 is the
349  same as ns=0, because the first ns columns would be zero below the
350  diagonal in that case anyway, so the HH transformation would be
351  null.]
352  For startup, all of the a priori information and state arrays may
353  be zero. That is, "no information" would imply that R and Z are zero,
354  as well as Rw and Zw. A priori information (covariance) and state
355  are handled by setting P = inverse(R)*transpose(inverse((R)), Z = R*X.
356  There are three ways to handle non-zero process noise covariance.
357  (1) If Q is the (known) a priori process noise covariance Q, then
358  set Q=Rw(-1)*Rw(-T), and G=1.
359  (2) Transform process noise covariance matrix to UDU form, Q=UDU^T,
360  then set G=U and Rw = (D)^-1/2.
361  (3) Take the sqrt of process noise covariance matrix Q, then set
362  G=this sqrt and Rw = 1. [2 and 3 have been tested.]
363  The routine applies a Householder transformation to a large
364  matrix formed by concatenation of the input matricies. Two preliminary
365  steps are to form Rd = R*PhiInv (stored in PhiInv) and -Rd*G (stored in
366  G) by matrix multiplication, and to set Rwx to the zero matrix.
367  Then the Householder transformation is applied to the following
368  matrix, dimensions are shown in ():
369  _ (ns) (n) (1) _ _ _
370  (ns) | Rw 0 Zw | ==> | Rw Rwx Zw |
371  (n) | -Rd*G Rd Z | ==> | 0 R Z | .
372  - - - -
373  The SRI matricies R and Rw remain upper triangular.
374 
375  For the programmer: after Rwx is set to zero, G is made into
376  -Rd*G and PhiInv is made into R*PhiInv, the transformation is applied
377  to the matrix:
378  _ (ns) (n) (1) _
379  (ns) | Rw Rwx Zw |
380  (n) | G PhiInv Z |
381  - -
382  then the (upper triangular) matrix R is copied out of PhiInv into R.
383  -------------------------------------------------------------------
384  The matrix Rwx is related to the sensitivity of the state
385  estimate to the unmodeled parameters in Zw. The sensitivity matrix
386  is Sen = -inverse(Rw)*Rwx,
387  where perturbation in model X =
388  Sen * diagonal(a priori sigmas of parameter uncertainties).
389  -------------------------------------------------------------------
390  The quantities Rw, Rwx and Zw on output are to be saved and used
391  in the sqrt information fixed interval smoother (SRIS), during the
392  backward filter process.
393  -------------------------------------------------------------------
394  Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
395  Estimation," Academic Press, 1977, pg 121.
396  ------------------------------------------------------------------- */
397  template <class T>
398  void SRIFilter::SrifTU(Matrix<T>& R, Vector<T>& Z, Matrix<T>& PhiInv,
399  Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
400  Matrix<T>& Rwx)
401  {
402  const T EPS = -T(1.e-200);
403  unsigned int n = R.rows(), ns = Rw.rows();
404  unsigned int i, j, k;
405  T sum, beta, delta, dum;
406 
407  if (PhiInv.rows() < n || PhiInv.cols() < n || G.rows() < n ||
408  G.cols() < ns || R.cols() != n || Rwx.rows() < ns || Rwx.cols() < n ||
409  Z.size() < n || Zw.size() < ns)
410  {
411  MatrixException me(
412  "Invalid input dimensions:\n R is " + asString<int>(R.rows()) +
413  "x" + asString<int>(R.cols()) + ", Z has length " +
414  asString<int>(Z.size()) + "\n PhiInv is " +
415  asString<int>(PhiInv.rows()) + "x" + asString<int>(PhiInv.cols()) +
416  "\n Rw is " + asString<int>(Rw.rows()) + "x" +
417  asString<int>(Rw.cols()) + "\n G is " + asString<int>(G.rows()) +
418  "x" + asString<int>(G.cols()) + "\n Zw has length " +
419  asString<int>(Zw.size()) + "\n Rwx is " +
420  asString<int>(Rwx.rows()) + "x" + asString<int>(Rwx.cols()));
421  GNSSTK_THROW(me);
422  }
423 
424  try
425  {
426  // initialize
427  Rwx = T(0);
428  PhiInv = R * PhiInv; // set PhiInv = Rd = R*PhiInv
429  G = -PhiInv * G;
430  /* fixed Matrix problem - unary minus should not return an l-value
431  G = -(PhiInv * G); // set G = -Rd*G */
432 
433  // temp
434  // Matrix <T> A;
435  // A = (Rw || Rwx || Zw) && (G || PhiInv || Z);
436  // cout << "SrifTU - :\n" << fixed << setw(10) << setprecision(5) << A
437  // << endl;
438 
439  //---------------------------------------------------------------
440  for (j = 0; j < ns; j++)
441  { // loop over first ns columns
442  sum = T(0);
443  for (i = 0; i < n; i++) // rows of -Rd*G
444  sum += G(i, j) * G(i, j);
445  dum = Rw(j, j);
446  sum += dum * dum;
447  sum = (dum > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
448  delta = dum - sum;
449  Rw(j, j) = sum;
450 
451  beta = sum * delta;
452  if (beta > EPS)
453  {
454  continue;
455  }
456  beta = T(1) / beta;
457 
458  /* apply jth Householder transformation
459  to submatrix below and right of (j,j) */
460  if (j + 1 < ns)
461  { // apply to G
462  for (k = j + 1; k < ns; k++)
463  { // columns to right of diagonal
464  sum = delta * Rw(j, k);
465  for (i = 0; i < n; i++) // rows of G
466  sum += G(i, j) * G(i, k);
467  if (sum == T(0))
468  {
469  continue;
470  }
471  sum *= beta;
472  Rw(j, k) += sum * delta;
473  for (i = 0; i < n; i++) // rows of G again
474  G(i, k) += sum * G(i, j);
475  }
476  }
477 
478  /* apply jth Householder transformation
479  to Rwx and PhiInv */
480  for (k = 0; k < n; k++)
481  { // columns of Rwx and PhiInv
482  sum = delta * Rwx(j, k);
483  for (i = 0; i < n; i++) // rows of PhiInv and G
484  sum += PhiInv(i, k) * G(i, j);
485  if (sum == T(0))
486  {
487  continue;
488  }
489  sum *= beta;
490  Rwx(j, k) += sum * delta;
491  for (i = 0; i < n; i++) // rows of PhiInv and G
492  PhiInv(i, k) += sum * G(i, j);
493  } // end loop over columns of Rwx and PhiInv
494 
495  /* apply jth Householder transformation
496  to Zw and Z */
497  sum = delta * Zw(j);
498  for (i = 0; i < n; i++) // rows of G and elements of Z
499  sum += Z(i) * G(i, j);
500  if (sum == T(0))
501  {
502  continue;
503  }
504  sum *= beta;
505  Zw(j) += sum * delta;
506  for (i = 0; i < n; i++) // rows of G and elements of Z
507  Z(i) += sum * G(i, j);
508  } // end loop over first ns columns
509 
510  //---------------------------------------------------------------
511  for (j = 0; j < n; j++)
512  { // loop over columns of Rwx and PhiInv
513  sum = T(0);
514  for (i = j + 1; i < n; i++) // rows of PhiInv
515  sum += PhiInv(i, j) * PhiInv(i, j);
516  dum = PhiInv(j, j);
517  sum += dum * dum;
518  sum = (dum > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
519  delta = dum - sum;
520  PhiInv(j, j) = sum;
521  beta = sum * delta;
522  if (beta > EPS)
523  {
524  continue;
525  }
526  beta = T(1) / beta;
527 
528  /* apply jth Householder transformation to columns of PhiInv on row
529  j */
530  for (k = j + 1; k < n; k++)
531  { // columns of PhiInv
532  sum = delta * PhiInv(j, k);
533  for (i = j + 1; i < n; i++)
534  sum += PhiInv(i, j) * PhiInv(i, k);
535  if (sum == T(0))
536  {
537  continue;
538  }
539  sum *= beta;
540  PhiInv(j, k) += sum * delta;
541  for (i = j + 1; i < n; i++)
542  PhiInv(i, k) += sum * PhiInv(i, j);
543  }
544 
545  // apply jth Householder transformation to Z
546  sum = delta * Z(j);
547  for (i = j + 1; i < n; i++)
548  sum += Z(i) * PhiInv(i, j);
549  if (sum == T(0))
550  {
551  continue;
552  }
553  sum *= beta;
554  Z(j) += sum * delta;
555  for (i = j + 1; i < n; i++)
556  Z(i) += sum * PhiInv(i, j);
557  } // end loop over cols of Rwx and PhiInv
558 
559  /* temp
560  A = (Rw || Rwx || Zw) && (G || PhiInv || Z);
561  cout << "SrifTU + :\n" << fixed << setw(10) << setprecision(5) << A
562  << endl; */
563 
564  // copy transformed R out of PhiInv
565  for (j = 0; j < n; j++)
566  for (i = 0; i <= j; i++)
567  R(i, j) = PhiInv(i, j);
568  }
569  catch (MatrixException& me)
570  {
571  GNSSTK_RETHROW(me);
572  }
573  } // end SrifTU
574 
575  //---------------------------------------------------------------------------------
576  /* Kalman smoother update.
577  This routine uses the Householder transformation to propagate the SRIF
578  state and covariance through a smoother (backward filter) step.
579  Input:
580  R A priori square root information (SRI) matrix (an N by N
581  upper triangular matrix)
582  z a priori SRIF state vector, an N vector (state is x, z = R*x).
583  Phi State transition matrix, an N by N matrix. Phi is destroyed on
584  output. Rw A priori square root information matrix for the process
585  noise, an Ns by Ns upper triangular matrix (which has
586  Ns(Ns+1)/2 elements).
587  G The N by Ns matrix associated with process noise. The
588  process noise covariance is GQGtrans where Qinverse
589  is Rw(trans)*Rw. G is destroyed on output.
590  Zw A priori 'state' associated with the process noise,
591  a vector with Ns elements. Zw is destroyed on output.
592  Rwx An Ns by N matrix. Rwx is destroyed on output.
593 
594  The inputs Rw,Zw,Rwx are the output of the SRIF time update, and these and
595  Phi and G are associated with the same timestep.
596 
597  Output:
598  The updated square root information matrix and SRIF smoothed state
599  (R,z).
600  All other inputs are trashed.
601 
602  Return values:
603  SrifSU returns void, but throws exceptions if the input matrices
604  or vectors have incompatible dimensions or incorrect types.
605 
606  Method:
607  The fixed interval square root information smoother (SRIS) is
608  composed of two Kalman filters, one identical with the square root
609  information filter (SRIF), the other similar but operating on the
610  data in reverse order and combining the current (smoothed) state
611  with elements output by the SRIF in its forward run and saved.
612  Thus a smoother is composed of a forward filter which saves all of
613  its output, followed by a backward filter which makes use of that
614  saved information.
615  This form of the SRIF backward filter algorithm is equivalent to the
616  Dyer-McReynolds SRIS algorithm, which uses less computer resources, but
617  propagates the state and covariance rather than the SRI (R,z). (As always,
618  at any point the state X and covariance P are related to the SRI by
619  X = R^-1 * z , P = R^-1 * R^-T.)
620  For startup of the backward filter, the state after the final
621  measurement update of the SRIF is given another time update, the
622  output of which is identified with the a priori values for the
623  backward filter. Backward filtering proceeds from there, the N+1st
624  point, toward the first point.
625 
626  In this implementation of the backward filter, the Householder
627  transformation is applied to the following matrix
628  (dimensions are shown in ()):
629 
630  _ (Ns) (N) (1) _ _ _
631  (Ns) | Rw+Rwx*G Rwx*Phi Zw | ==> | Rw Rwx Zw |
632  (N) | R*G R*Phi z | ==> | 0 R z | .
633  - - - -
634  The SRI matricies R and Rw remain upper triangular.
635 
636  For the programmer: First create an NsXNs matrix A, then
637  Rw+Rwx*G -> A, Rwx*Phi -> Rwx, R*Phi -> Phi, and R*G -> G, and
638  the transformation is applied to the matrix:
639 
640  _ (Ns) (N) (1) _
641  (Ns) | A Rwx Zw |
642  (N) | G Phi z |
643  - -
644  then the (upper triangular) matrix R is copied out of Phi into R.
645 
646  Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
647  Estimation," Academic Press, 1977, pg 216. */
648  template <class T>
649  void SRIFilter::SrifSU(Matrix<T>& R, Vector<T>& Z, Matrix<T>& Phi,
650  Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
651  Matrix<T>& Rwx)
652  {
653  unsigned int N = R.rows(), Ns = Rw.rows();
654 
655  if (Phi.rows() < N || Phi.cols() < N || G.rows() < N || G.cols() < Ns ||
656  R.cols() != N || Rwx.rows() < Ns || Rwx.cols() < N || Z.size() < N ||
657  Zw.size() < Ns)
658  {
659  MatrixException me(
660  "Invalid input dimensions:\n R is " + asString<int>(R.rows()) +
661  "x" + asString<int>(R.cols()) + ", Z has length " +
662  asString<int>(Z.size()) + "\n Phi is " +
663  asString<int>(Phi.rows()) + "x" + asString<int>(Phi.cols()) +
664  "\n Rw is " + asString<int>(Rw.rows()) + "x" +
665  asString<int>(Rw.cols()) + "\n G is " + asString<int>(G.rows()) +
666  "x" + asString<int>(G.cols()) + "\n Zw has length " +
667  asString<int>(Zw.size()) + "\n Rwx is " +
668  asString<int>(Rwx.rows()) + "x" + asString<int>(Rwx.cols()));
669  GNSSTK_THROW(me);
670  }
671 
672  const T EPS = -T(1.e-200);
673  size_t i, j, k;
674  T sum, beta, delta, diag;
675 
676  try
677  {
678  // Rw+Rwx*G -> A
679  Matrix<T> A;
680  A = Rw + Rwx * G;
681  Rwx = Rwx * Phi;
682  Phi = R * Phi;
683  G = R * G;
684 
685  //-----------------------------------------
686  // HouseHolder Transformation
687 
688  // Loop over first Ns columns
689  for (j = 0; j < Ns; j++)
690  { // columns of A
691  sum = T(0);
692  for (i = j + 1; i < Ns; i++)
693  { // rows i below diagonal in A
694  sum += A(i, j) * A(i, j);
695  }
696  for (i = 0; i < N; i++)
697  { // all rows i in G
698  sum += G(i, j) * G(i, j);
699  }
700 
701  diag = A(j, j);
702  sum += diag * diag;
703  sum = (diag > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
704  delta = diag - sum;
705  A(j, j) = sum;
706  beta = sum * delta;
707  if (beta > EPS)
708  {
709  continue;
710  }
711  beta = T(1) / beta;
712 
713  // apply jth HH trans to submatrix below and right of j,j
714  for (k = j + 1; k < Ns; k++)
715  { // cols to right of diag
716  sum = delta * A(j, k);
717  for (i = j + 1; i < Ns; i++)
718  { // rows of A below diagonal
719  sum += A(i, j) * A(i, k);
720  }
721  for (i = 0; i < N; i++)
722  { // all rows of G
723  sum += G(i, j) * G(i, k);
724  }
725  if (sum == T(0))
726  {
727  continue;
728  }
729  sum *= beta;
730  //------------------------------------------
731  A(j, k) += sum * delta;
732 
733  for (i = j + 1; i < Ns; i++)
734  { // rows of A > j (same loops again)
735  A(i, k) += sum * A(i, j);
736  }
737  for (i = 0; i < N; i++)
738  { // all rows of G (again)
739  G(i, k) += sum * G(i, j);
740  }
741  }
742 
743  // apply jth HH trans to Rwx and Phi sub-matrices
744  for (k = 0; k < N; k++)
745  { // all columns of Rwx / Phi
746  sum = delta * Rwx(j, k);
747  for (i = j + 1; i < Ns; i++)
748  { // rows of Rwx below j
749  sum += A(i, j) * Rwx(i, k);
750  }
751  for (i = 0; i < N; i++)
752  { // all rows of Phi
753  sum += G(i, j) * Phi(i, k);
754  }
755  if (sum == T(0))
756  {
757  continue;
758  }
759  sum *= beta;
760  Rwx(j, k) += sum * delta;
761  for (i = j + 1; i < Ns; i++)
762  { // rows of Rwx below j (again)
763  Rwx(i, k) += sum * A(i, j);
764  }
765  for (i = 0; i < N; i++)
766  { // all rows of Phi (again)
767  Phi(i, k) += sum * G(i, j);
768  }
769  }
770 
771  // apply jth HH trans to Zw and Z
772  sum = delta * Zw(j);
773  for (i = j + 1; i < Ns; i++)
774  { // rows (elements) of Zw below j
775  sum += A(i, j) * Zw(i);
776  }
777  for (i = 0; i < N; i++)
778  { // all rows (elements) of Z
779  sum += Z(i) * G(i, j);
780  }
781  if (sum == T(0))
782  {
783  continue;
784  }
785  sum *= beta;
786  Zw(j) += sum * delta;
787  for (i = j + 1; i < Ns; i++)
788  { // rows of Zw below j (again)
789  Zw(i) += sum * A(i, j);
790  }
791  for (i = 0; i < N; i++)
792  { // all rows of Z (again)
793  Z(i) += sum * G(i, j);
794  }
795  }
796 
797  // Loop over columns past the Ns block: all of Rwx and Phi
798  for (j = 0; j < N; j++)
799  {
800  sum = T(0);
801  for (i = j + 1; i < N; i++)
802  { // rows of Phi at and below j
803  sum += Phi(i, j) * Phi(i, j);
804  }
805  diag = Phi(j, j);
806  sum += diag * diag;
807  sum = (diag > T(0) ? -T(1) : T(1)) * ::sqrt(sum);
808  delta = diag - sum;
809  Phi(j, j) = sum;
810  beta = sum * delta;
811  if (beta > EPS)
812  {
813  continue;
814  }
815  beta = T(1) / beta;
816 
817  // apply HH trans to Phi sub-block below and right of j,j
818  for (k = j + 1; k < N; k++)
819  { // columns k > j
820  sum = delta * Phi(j, k);
821  for (i = j + 1; i < N; i++)
822  { // rows below j
823  sum += Phi(i, j) * Phi(i, k);
824  }
825  if (sum == T(0))
826  {
827  continue;
828  }
829  sum *= beta;
830  Phi(j, k) += sum * delta;
831  for (i = j + 1; i < N; i++)
832  { // rows below j (again)
833  Phi(i, k) += sum * Phi(i, j);
834  }
835  }
836  // Now apply to the Z column
837  sum = delta * Z(j);
838  for (i = j + 1; i < N; i++)
839  { // rows of Z below j
840  sum += Z(i) * Phi(i, j);
841  }
842  if (sum == T(0))
843  {
844  continue;
845  }
846  sum *= beta;
847  Z(j) += sum * delta;
848  for (i = j + 1; i < N; i++)
849  { // rows of Z below j (again)
850  Z(i) += sum * Phi(i, j);
851  }
852  }
853  //------------------------------
854  // Transformation finished
855 
856  //-------------------------------------
857  // copy transformed R out of Phi into R
858  R = T(0);
859  for (j = 0; j < N; j++)
860  {
861  for (i = 0; i <= j; i++)
862  {
863  R(i, j) = Phi(i, j);
864  }
865  }
866  }
867  catch (Exception& e)
868  {
869  GNSSTK_RETHROW(e);
870  }
871  } // end SrifSU
872 
873  //------------------------------------------------------------------------------
874  /* Covariance/State version of the Kalman smoother update (Dyer-McReynolds).
875  This routine implements the Dyer-McReynolds form of the state and
876  covariance recursions which constitute the backward filter of the Square
877  Root Information Smoother.
878 
879  Input: (assume N and Ns are greater than zero)
880  Vector X(N) A priori state, derived from SRI (R*X=Z)
881  Matrix P(N,N) A priori covariance, derived from SRI
882  (P=R^-1*R^-T)
883  Matrix Rw(Ns,Ns) Process noise covariance (UT), output of SRIF TU
884  Matrix Rwx(Ns,N) PN 'cross term', output of SRIF TU
885  Vector Zw(Ns) Process noise state, output of SRIF TU
886  Matrix Phinv(N,N) Inverse of state transition, saved at SRIF TU
887  Matrix G(N,Ns) Noise coupling matrix, saved at SRIF TU
888  Output:
889  Updated X and P. The other inputs are trashed.
890 
891  Method:
892  The fixed interval square root information smoother (SRIS) is
893  composed of two Kalman filters, one identical with the square root
894  information filter (SRIF), the other similar but operating on the
895  data in reverse order and combining the current (smoothed) state
896  with elements output by the SRIF in its forward run and saved.
897  Thus a smoother is composed of a forward filter which saves all of
898  its output, followed by a backward filter which makes use of that
899  saved information.
900  This form of the SRIS algorithm is equivalent to the SRIS backward
901  filter Householder transformation algorithm, but uses less computer
902  resources. It is not necessary to update both the state and the
903  covariance, although doing both at once is less expensive than
904  doing them separately. (This routine does both.)
905  For startup of the backward filter, the state after the final
906  measurement update of the SRIF is given another time update, the
907  output of which is identified with the a priori values for the
908  backward filter. Backward filtering proceeds from there, the N+1st
909  point, toward the first point.
910 
911  Ref: Bierman, G.J. "Factorization Methods for Discrete Sequential
912  Estimation," Academic Press, 1977, pg 216. */
913  template <class T>
914  void SRIFilter::SrifSU_DM(Matrix<T>& P, Vector<T>& X, Matrix<T>& Phinv,
915  Matrix<T>& Rw, Matrix<T>& G, Vector<T>& Zw,
916  Matrix<T>& Rwx)
917  {
918  unsigned int N = P.rows(), Ns = Rw.rows();
919 
920  if (P.cols() != P.rows() || X.size() != N || Rwx.cols() != N ||
921  Zw.size() != Ns || Rwx.rows() != Ns || Rwx.cols() != N ||
922  Phinv.rows() != N || Phinv.cols() != N || G.rows() != N ||
923  G.cols() != Ns)
924  {
925  MatrixException me(
926  "Invalid input dimensions:\n P is " + asString<int>(P.rows()) +
927  "x" + asString<int>(P.cols()) + ", X has length " +
928  asString<int>(X.size()) + "\n Phinv is " +
929  asString<int>(Phinv.rows()) + "x" + asString<int>(Phinv.cols()) +
930  "\n Rw is " + asString<int>(Rw.rows()) + "x" +
931  asString<int>(Rw.cols()) + "\n G is " + asString<int>(G.rows()) +
932  "x" + asString<int>(G.cols()) + "\n Zw has length " +
933  asString<int>(Zw.size()) + "\n Rwx is " +
934  asString<int>(Rwx.rows()) + "x" + asString<int>(Rwx.cols()));
935  GNSSTK_THROW(me);
936  }
937 
938  try
939  {
940  G = G * inverseLUD(Rw);
941  Matrix<T> F;
942  F = ident<T>(N) + G * Rwx;
943  // update X
944  Vector<T> C;
945  C = F * X - G * Zw;
946  X = Phinv * C;
947  // update P
948  P = F * P * transpose(F) + G * transpose(G);
949  P = Phinv * P * transpose(Phinv);
950  }
951  catch (Exception& e)
952  {
953  GNSSTK_RETHROW(e);
954  }
955  } // end SrifSU_DM
956 
957  // Modification for case with control vector: Xj+1 = Phi*Xj + Gwj + u
958  template <class T>
959  void DMsmootherUpdateWithControl(Matrix<double>& P, Vector<double>& X,
960  Matrix<double>& Phinv, Matrix<double>& Rw,
961  Matrix<double>& G, Vector<double>& Zw,
962  Matrix<double>& Rwx, Vector<double>& U)
963  {
964  unsigned int N = P.rows(), Ns = Rw.rows();
965 
966  if (P.cols() != P.rows() || X.size() != N || Rwx.cols() != N ||
967  Zw.size() != Ns || Rwx.rows() != Ns || Rwx.cols() != N ||
968  Phinv.rows() != N || Phinv.cols() != N || G.rows() != N ||
969  G.cols() != Ns || U.size() != N)
970  {
971  MatrixException me(
972  "Invalid input dimensions:\n P is " + asString<int>(P.rows()) +
973  "x" + asString<int>(P.cols()) + ", X has length " +
974  asString<int>(X.size()) + "\n Phinv is " +
975  asString<int>(Phinv.rows()) + "x" + asString<int>(Phinv.cols()) +
976  "\n Rw is " + asString<int>(Rw.rows()) + "x" +
977  asString<int>(Rw.cols()) + "\n G is " + asString<int>(G.rows()) +
978  "x" + asString<int>(G.cols()) + "\n Zw has length " +
979  asString<int>(Zw.size()) + "\n Rwx is " +
980  asString<int>(Rwx.rows()) + "x" + asString<int>(Rwx.cols()) +
981  "\n U has length " + asString<int>(U.size()));
982  GNSSTK_THROW(me);
983  }
984 
985  try
986  {
987  G = G * inverseLUD(Rw);
988  Matrix<T> F;
989  F = ident<T>(N) + G * Rwx;
990  // update X
991  Vector<T> C;
992  C = F * X - G * Zw - U;
993  X = Phinv * C;
994  // update P
995  P = F * P * transpose(F) + G * transpose(G);
996  P = Phinv * P * transpose(Phinv);
997  P += outer(U, U);
998  }
999  catch (Exception& e)
1000  {
1001  GNSSTK_RETHROW(e);
1002  }
1003  } // end DMsmootherUpdateWithControl
1004 
1005  //---------------------------------------------------------------------------------
1006 } // end namespace gnsstk
gnsstk::SRINullMatrix
const Matrix< double > SRINullMatrix
constant (empty) Matrix used for default input arguments
Definition: SRI.cpp:69
gnsstk::inverseLUD
Matrix< T > inverseLUD(const ConstMatrixBase< T, BaseClass > &m)
Definition: MatrixOperators.hpp:591
gnsstk::lowerCholesky
SparseMatrix< T > lowerCholesky(const SparseMatrix< T > &A)
Definition: SparseMatrix.hpp:2065
StringUtils.hpp
gnsstk::SparseMatrix::colCopy
SparseVector< T > colCopy(const unsigned int j) const
return col j of this SparseMatrix as a SparseVector
Definition: SparseMatrix.hpp:1676
gnsstk::sum
T sum(const ConstVectorBase< T, BaseClass > &l)
Definition: VectorBaseOperators.hpp:84
gnsstk::Matrix::cols
size_t cols() const
The number of columns in the matrix.
Definition: Matrix.hpp:167
gnsstk::Matrix::rows
size_t rows() const
The number of rows in the matrix.
Definition: Matrix.hpp:165
gnsstk::SRI::R
Matrix< double > R
Information matrix, an upper triangular (square) matrix.
Definition: SRI.hpp:601
gnsstk::SRIFilter
Definition: SRIFilter.hpp:93
gnsstk
For Sinex::InputHistory.
Definition: BasicFramework.cpp:50
gnsstk::diag
Matrix< T > diag(const ConstMatrixBase< T, BaseClass > &m)
Definition: MatrixOperators.hpp:414
gnsstk::SparseMatrix::rows
unsigned int rows() const
get number of rows - of the real Matrix, not the data array
Definition: SparseMatrix.hpp:460
gnsstk::SRI::Z
Vector< double > Z
SRI state vector, of length equal to the dimension (row and col) of R.
Definition: SRI.hpp:604
gnsstk::Exception
Definition: Exception.hpp:151
gnsstk::SrifMU
void SrifMU(Matrix< T > &R, Vector< T > &Z, SparseMatrix< T > &A, const unsigned int M)
Definition: SparseMatrix.hpp:2467
gnsstk::SRINullSparseMatrix
const SparseMatrix< double > SRINullSparseMatrix
constant (empty) SparseMatrix used for default input arguments
Definition: SRI.cpp:70
gnsstk::transpose
SparseMatrix< T > transpose(const SparseMatrix< T > &M)
transpose
Definition: SparseMatrix.hpp:829
gnsstk::Matrix< double >
gnsstk::inverseLT
SparseMatrix< T > inverseLT(const SparseMatrix< T > &LT, T *ptrSmall, T *ptrBig)
Compute inverse of lower-triangular SparseMatrix.
Definition: SparseMatrix.hpp:2154
gnsstk::Namelist::size
unsigned int size() const
return the size of the list.
Definition: Namelist.hpp:408
gnsstk::beta
double beta(double x, double y)
Definition: SpecialFuncs.cpp:204
gnsstk::SparseMatrix::cols
unsigned int cols() const
get number of columns - of the real Matrix, not the data array
Definition: SparseMatrix.hpp:463
gnsstk::SRI::names
Namelist names
Namelist parallel to R and Z, labelling the elements of the state vector.
Definition: SRI.hpp:607
GNSSTK_RETHROW
#define GNSSTK_RETHROW(exc)
Definition: Exception.hpp:369
gnsstk::Vector< double >
gnsstk::TrackingCode::P
@ P
Legacy GPS precise code.
RobustStats.hpp
gnsstk::Vector::size
size_t size() const
STL size.
Definition: Vector.hpp:207
std
Definition: Angle.hpp:142
gnsstk::DMsmootherUpdateWithControl
void DMsmootherUpdateWithControl(Matrix< double > &P, Vector< double > &X, Matrix< double > &Phinv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx, Vector< double > &U)
Definition: SRIFilter.cpp:959
SRIFilter.hpp
GNSSTK_THROW
#define GNSSTK_THROW(exc)
Definition: Exception.hpp:366
gnsstk::Namelist
Definition: Namelist.hpp:287
gnsstk::outer
Matrix< T > outer(const ConstVectorBase< T, BaseClass > &v, const ConstVectorBase< T, BaseClass > &w)
Definition: MatrixOperators.hpp:933
gnsstk::SparseMatrix
Definition: SparseMatrix.hpp:53

gnsstk
Author(s):
autogenerated on Wed Oct 25 2023 02:40:41