44#include "EST_kalman.h"
66 if(!kalman_filter_param_check(
x,P,
Q,
R,A,H,z))
68 cerr <<
"Kalman filter parameters inconsistent !" <<
endl;
101 cerr <<
" H * P * Ht + R is singular !" <<
endl;
104 cerr <<
"Matrix inversion failed for an unknown reason !" <<
endl;
109 x = add(
x,
K * subtract(z,H *
x));
140 if(!kalman_filter_param_check(
x,
Pinv,
Q,
Rinv,A,H,z))
142 cerr <<
"Kalman filter parameters inconsistent !" <<
endl;
168 cerr <<
"Matrix inversion failed for an unknown reason !" <<
endl;
178 x = add(
x,
K * subtract(z,H*
x));
188 cerr <<
"Pinv is singular !" <<
endl;
191 cerr <<
"Matrix inversion failed for an unknown reason !" <<
endl;
224 cerr <<
"No state or measurements !!" <<
endl;
234 cerr <<
"P, or Pinv, must be a symmetrical square matrix of the same dimension" <<
endl;
235 cerr <<
"as the state vector, x" <<
endl;
243 cerr <<
"Q must be a symmetrical square matrix of the same dimension" <<
endl;
244 cerr <<
"as the state vector, x" <<
endl;
252 cerr <<
"R, or Rinv, must be a symmetrical square matrix of the same dimension" <<
endl;
253 cerr <<
"as the measurement vector, z" <<
endl;
260 cerr <<
"A must be a square matrix of the same dimension" <<
endl;
261 cerr <<
"as the state vector, x" <<
endl;
268 cerr <<
"H must have dimensions to fit z = Hx" <<
endl;
int num_columns() const
return number of columns
int num_rows() const
return number of rows
INLINE int length() const
number of items in vector.