contrib/oul/ouel/kalman_filter.cxx
Go to the documentation of this file.
```00001 // This is oul/ouel/kalman_filter.cxx
00002 #include "kalman_filter.h"
00003 //:
00004 // \file
00005 // \brief kalman_filter.cc: A linear Kalman filter class
00006 //
00007 // Implementation of a linear Kalman filter. We want to estimate the
00008 // value of a time-varying signal from a measurement and predict the
00009 // next value of the signal.
00010 //
00011 // The relationship between signal and measurement is:
00012 // z(k)        =  H(k)    *    x(k)     +    v(k)
00013 // measurement    relation     signal        noise
00014 //                matrix
00015 //
00016 // The relationship between signals at different times is:
00017 // x(k+1)      =  A(k)    *    x(k)   +   B(k)   *   u(k)   +   w(k)
00018 // signal         relation     signal     control    control    noise
00019 //                matrix                  matrix     signal
00020 //
00021 // Copyright (c) 1999 Brendan McCane
00022 // University of Otago, Dunedin, New Zealand
00023 // Reproduction rights limited as described in the COPYRIGHT file.
00024 //----------------------------------------------------------------------
00025
00026 #include <vcl_cstdlib.h>
00027 #include <vcl_iostream.h>
00028
00029 #include <vnl/algo/vnl_svd.h>
00030
00031 //----------------------------------------------------------------------
00032 //: Constructor
00033 //
00034 // Initialises the necessary parameters and matrices
00035 //
00036 // \param ns  number of dimensions in signal
00037 // \param nm  number of dimensions in measurement
00038 // \param nc  number of dimensions in control input
00039 //
00040 // \param A   the ns*ns matrix relating signal at time k to signal at time k+1.
00041 //
00042 // \param H   the nm*ns matrix relating the measurement to the signal.
00043 //
00044 // \param B   the nc*ns matrix relating the signal to the control input.
00045 //   I'm not sure what this is used for, but I've included it for completeness.
00046 //
00047 // \param x_initial  the initial ns*1 estimate of the signal.
00048 //
00049 // \param p_initial  the initial error variance vector.
00050 //        P = p_initial.transpose()*p_initial.
00051 //
00052 // \todo   under development
00053 // \author Brendan McCane
00054 //----------------------------------------------------------------------
00055
00056 KalmanFilter::KalmanFilter(
00057   unsigned int ns, unsigned int nm, unsigned int nc,
00058   const vnl_matrix<double> &Ai,
00059   const vnl_matrix<double> &Hi,
00060   const vnl_matrix<double> &Bi,
00061   const vnl_matrix<double> &z_initial,
00062   const vnl_matrix<double> &x_initial,
00063   const vnl_matrix<double> &Pi
00064   ):
00065   // initialise all the matrices etc
00066   num_signal_dimensions(ns), num_measurement_dimensions(nm),
00067   num_control_dimensions(nc), A(Ai), H(Hi), B(Bi),
00068   x(x_initial), x_pred(x_initial), z(z_initial),
00069   P(Pi), K(ns, nm)
00070 {
00071   // do some size checking
00072   if ((A.rows()!=num_signal_dimensions)||
00073     (A.cols()!=num_signal_dimensions))
00074   {
00075     vcl_cerr << "Error in Kalman constructor:\n"
00076              << "\tMatrix A must be of size ns*ns\n";
00077     vcl_exit(-1);
00078   }
00079
00080   if ((H.rows()!=num_measurement_dimensions)||
00081     (H.cols()!=num_signal_dimensions))
00082   {
00083     vcl_cerr << "Error in Kalman constructor:\n"
00084              << "\tMatrix H must be of size nm*ns\n";
00085     vcl_exit(-1);
00086   }
00087
00088   if ((B.cols()!=num_control_dimensions)||
00089     (B.rows()!=num_signal_dimensions))
00090   {
00091     vcl_cerr << "Error in Kalman constructor:\n"
00092              << "\tMatrix B must be of size ns*nc\n";
00093     vcl_exit(-1);
00094   }
00095
00096   if ((x_pred.rows()!=num_signal_dimensions)||
00097     (x_pred.cols()!=1))
00098   {
00099     vcl_cerr << "Error in Kalman constructor:\n"
00100              << "\tMatrix x must be of size ns*1\n";
00101     vcl_exit(-1);
00102   }
00103
00104   if ((Pi.rows()!=num_signal_dimensions)||
00105     (Pi.cols()!=num_signal_dimensions))
00106   {
00107     vcl_cerr << "Error in Kalman constructor:\n"
00108              << "\tMatrix p_initial must be of size ns*1\n";
00109     vcl_exit(-1);
00110   }
00111 }
00112
00113
00114 //----------------------------------------------------------------------
00115 //: Constructor
00116 //
00117 // Initialises the necessary parameters and matrices
00118 //
00119 // \param ns  number of dimensions in signal
00120 // \param nm  number of dimensions in measurement
00121 //
00122 // \param A   the ns*ns matrix relating signal at time k to signal at time k+1.
00123 //
00124 // \param H   the nm*ns matrix relating the measurement to the signal.
00125 //
00126 // \param x_initial  the initial ns*1 estimate of the signal.
00127 //
00128 // \param p_initial  the initial error variance vector.
00129 //        P = p_initial.transpose()*p_initial.
00130 //
00131 // \todo   under development
00132 // \author Brendan McCane
00133 //----------------------------------------------------------------------
00134 KalmanFilter::KalmanFilter(
00135   unsigned int ns, unsigned int nm,
00136   const vnl_matrix<double> &Ai,
00137   const vnl_matrix<double> &Hi,
00138   const vnl_matrix<double> &z_initial,
00139   const vnl_matrix<double> &x_initial,
00140   const vnl_matrix<double> &Pi
00141   ):
00142   // initialise all the matrices etc
00143   num_signal_dimensions(ns), num_measurement_dimensions(nm),
00144   num_control_dimensions(0), A(Ai), H(Hi),
00145   x(x_initial), x_pred(x_initial), z(z_initial),
00146   P(Pi), K(ns, nm)
00147 {
00148   // do some size checking
00149   if ((A.rows()!=num_signal_dimensions)||
00150     (A.cols()!=num_signal_dimensions))
00151   {
00152     vcl_cerr << "Error in Kalman constructor:\n"
00153              << "\tMatrix A must be of size ns*ns\n";
00154     vcl_exit(-1);
00155   }
00156
00157   if ((H.rows()!=num_measurement_dimensions)||
00158     (H.cols()!=num_signal_dimensions))
00159   {
00160     vcl_cerr << "Error in Kalman constructor:\n"
00161              << "\tMatrix H must be of size nm*ns\n";
00162     vcl_exit(-1);
00163   }
00164
00165   if ((x_pred.rows()!=num_signal_dimensions)||
00166     (x_pred.cols()!=1))
00167   {
00168     vcl_cerr << "Error in Kalman constructor:\n"
00169              << "\tMatrix x must be of size ns*1\n";
00170     vcl_exit(-1);
00171   }
00172
00173   if ((Pi.rows()!=num_signal_dimensions)||
00174     (Pi.cols()!=num_signal_dimensions))
00175   {
00176     vcl_cerr << "Error in Kalman constructor:\n"
00177              << "\tMatrix p_initial must be of size ns*1\n";
00178     vcl_exit(-1);
00179   }
00180 }
00181
00182 //----------------------------------------------------------------------
00183 //: measurement_update
00184 //
00185 // Calculate the Kalman gain, update the signal estimate from the last
00186 // time point using the previous estimate and the new measurement, and
00187 // update the error covariance matrix.
00188 //
00189 // \param zk the new measurement
00190 // \param Rk the measurement error covariance matrix
00191 //
00192 // \todo   under development
00193 // \author Brendan McCane
00194 //----------------------------------------------------------------------
00195
00196 void KalmanFilter::measurement_update(const vnl_matrix<double> &zk,
00197                                       const vnl_matrix<double> &Rk)
00198 {
00199   // do some checks
00200   if ((zk.rows()!=num_measurement_dimensions)||
00201     (zk.cols()!=1))
00202   {
00203     vcl_cerr << "Error in Kalman measurement_update\n"
00204              << "\tzk must have dimensions nm*1\n";
00205     vcl_exit(-1);
00206   }
00207
00208   if ((Rk.rows()!=num_measurement_dimensions)||
00209     (Rk.cols()!=num_measurement_dimensions))
00210   {
00211     vcl_cerr << "Error in Kalman measurement_update\n"
00212              << "\tRk must have dimensions nm*nm\n";
00213     vcl_exit(-1);
00214   }
00215
00216   this->z = zk;
00217
00218   // first calculate the Kalman gain
00219   vnl_matrix<double> Tmp(Rk);
00220
00221   Tmp += H*P*H.transpose();
00222
00223   vnl_svd<double> Tmp2(Tmp);    // Singular value decomp inverse
00224
00225   K = P*H.transpose()*Tmp2.inverse();
00226
00227   // update the signal estimate using the previous estimate plus the
00228   // measurement.
00229   // since zk = H*x + v, zk-H*x is an error estimate
00230   x = x_pred + K*(zk-H*x_pred);
00231
00232   // update the error covariance
00233   vnl_matrix<double> ident(P);
00234   ident.set_identity();
00235
00236   P = (ident - K*H)*P;
00237 }
00238
00239 //----------------------------------------------------------------------
00240 //: predict
00241 //
00242 // Calculate the next state given the current estimate and project the
00243 // error covariance matrix ahead in time as well. In this case, I am
00244 // assuming there is no control input.
00245 //
00246 // \param Qk  the process error covariance matrix
00247 //
00248 // \todo   under development
00249 // \author Brendan McCane
00250 //----------------------------------------------------------------------
00251
00252 vnl_matrix<double>
00253 KalmanFilter::predict(const vnl_matrix<double> &Qk)
00254 {
00255   // do some checks
00256   if ((Qk.rows()!=num_signal_dimensions)||
00257     (Qk.cols()!=num_signal_dimensions))
00258   {
00259     vcl_cerr << "Error in Kalman predict\n"
00260              << "\tQk must have dimensions ns*ns\n";
00261     vcl_exit(-1);
00262   }
00263
00264   // estimate the new state
00265   x_pred = A*x;
00266
00267   // estimate the new error covariance matrix
00268   P = A*P*A.transpose() + Qk;
00269
00270   return x_pred;
00271 }
00272
00273 //----------------------------------------------------------------------
00274 //: predict
00275 //
00276 // Calculate the next state given the current estimate and project the
00277 // error covariance matrix ahead in time as well.
00278 //
00279 // \param Qk  the process error covariance matrix
00280 // \param uk  the control input
00281 //
00282 // \todo   under development
00283 // \author Brendan McCane
00284 //----------------------------------------------------------------------
00285
00286 vnl_matrix<double>
00287 KalmanFilter::predict(const vnl_matrix<double> &Qk,
00288                       const vnl_matrix<double> &uk)
00289 {
00290   // do some checks
00291   if ((Qk.rows()!=num_signal_dimensions)||
00292     (Qk.cols()!=num_signal_dimensions))
00293   {
00294     vcl_cerr << "Error in Kalman predict\n"
00295              << "\tQk must have dimensions ns*ns\n";
00296     vcl_exit(-1);
00297   }
00298
00299   if ((uk.rows()!=num_control_dimensions)||
00300     (uk.cols()!=1))
00301   {
00302     vcl_cerr << "Error in Kalman predict\n"
00303              << "\tuk must have dimensions nc*1\n";
00304     vcl_exit(-1);
00305   }
00306
00307   // estimate the new state
00308   x_pred = A*x + B*uk;
00309
00310   // estimate the new error covariance matrix
00311   P = A*P*A.transpose() + Qk;
00312
00313   return x_pred;
00314 }
00315
00316 //----------------------------------------------------------------------
00317 //: update_predict
00318 //
00319 // A wrapper function to both update the measurement and predict the
00320 // new state.
00321 //
00322 // \param zk  the new measurement
00323 // \param Rk  the measurement error covariance matrix
00324 // \param Qk  the process error covariance matrix
00325 //
00326 // \todo   under development
00327 // \author Brendan McCane
00328 //----------------------------------------------------------------------
00329
00330 vnl_matrix<double> KalmanFilter::update_predict
00331 (
00332   const vnl_matrix<double> &zk,
00333   const vnl_matrix<double> &Rk,
00334   const vnl_matrix<double> &Qk
00335 )
00336 {
00337   measurement_update(zk, Rk);
00338   return predict(Qk);
00339 }
00340
00341 //----------------------------------------------------------------------
00342 //: output operator
00343 //
00344 // \param kf   the Kalman filter to output
00345 //
00346 // \todo   under development
00347 // \author Brendan McCane
00348 //----------------------------------------------------------------------
00349
00350 vcl_ostream &operator<<(vcl_ostream &os, const KalmanFilter &kf)
00351 {
00352   os << "Current state of Kalman Filter is:\n"
00353      << "estimate = " << kf.x.transpose()
00354      << "prediction = " << kf.x_pred.transpose()
00355      << "measurement = " << kf.z.transpose()
00356      << "error covariance =\n" << kf.P
00357      << "Kalman gain =\n" << kf.K
00358      << "A =\n" << kf.A
00359      << "H =\n" << kf.H;
00360   if (kf.num_control_dimensions>0)
00361     os << "B =\n" << kf.B;
00362   return os;
00363 }
```