Loading...
Searching...
No Matches
EST_kalman.cc
/*************************************************************************/
/* */
/* Centre for Speech Technology Research */
/* University of Edinburgh, UK */
/* Copyright (c) 1995,1996 */
/* All Rights Reserved. */
/* */
/* Permission is hereby granted, free of charge, to use and distribute */
/* this software and its documentation without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of this work, and to */
/* permit persons to whom this work is furnished to do so, subject to */
/* the following conditions: */
/* 1. The code must retain the above copyright notice, this list of */
/* conditions and the following disclaimer. */
/* 2. Any modifications must be clearly marked as such. */
/* 3. Original authors' names are not deleted. */
/* 4. The authors' names are not used to endorse or promote products */
/* derived from this software without specific prior written */
/* permission. */
/* */
/* THE UNIVERSITY OF EDINBURGH AND THE CONTRIBUTORS TO THIS WORK */
/* DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING */
/* ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT */
/* SHALL THE UNIVERSITY OF EDINBURGH NOR THE CONTRIBUTORS BE LIABLE */
/* FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES */
/* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN */
/* AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, */
/* ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF */
/* THIS SOFTWARE. */
/* */
/*************************************************************************/
/* Author : Simon King */
/* Date : June 1998 */
/*-----------------------------------------------------------------------*/
/* Kalman filtering, i.e. linear model fitting */
/* */
/*=======================================================================*/
#include <cstdlib>
#include <cstdio>
#include <fstream>
#include "EST.h"
#include "EST_kalman.h"
static bool kalman_filter_param_check(EST_FVector &x,
EST_FMatrix &P,
EST_FMatrix &Q,
EST_FMatrix &R,
EST_FMatrix &A,
EST_FMatrix &H,
EST_FVector &z);
bool kalman_filter(EST_FVector &x,
EST_FMatrix &P,
EST_FMatrix &Q,
EST_FMatrix &R,
EST_FMatrix &A,
EST_FMatrix &H,
EST_FVector &z)
{
// see kalman.h for meaning of args
if(!kalman_filter_param_check(x,P,Q,R,A,H,z))
{
cerr << "Kalman filter parameters inconsistent !" << endl;
return FALSE;
}
EST_FMatrix K,I,At,Ht,PHt,HPHt_R,HPHt_R_inv;
int state_dim=x.length();
int singularity;
eye(I,state_dim);
transpose(A,At);
cerr << "predict" << endl;
// predict
// =======
// if A is the identity matrix we could speed this up a LOT
x = A * x;
P = A * P * At + Q;
cerr << "correct" << endl;
// correct
// =======
// T T -1
// K = P H ( H P H + R )
transpose(H,Ht);
PHt = P * Ht;
HPHt_R=(H * PHt) + R;
if(!inverse( HPHt_R , HPHt_R_inv, singularity))
{
if(singularity != -1)
{
cerr << " H * P * Ht + R is singular !" << endl;
return FALSE;
}
cerr << "Matrix inversion failed for an unknown reason !" << endl;
return FALSE;
}
K = PHt * HPHt_R_inv;
x = add(x, K * subtract(z,H * x));
P = (I - K * H) * P;
// try and remedy numerical errors
symmetrize(P);
//cerr << "done" << endl;
return TRUE;
}
bool kalman_filter_Pinv(EST_FVector &x,
EST_FMatrix &Pinv,
EST_FMatrix &Q,
EST_FMatrix &Rinv,
EST_FMatrix &A,
EST_FMatrix &H,
EST_FVector &z)
{
// a different formulation, using the inverse
// covariance matrix, and a more stable update
// equation
// from:
// Intro. to Random Signals and Kalman Applied Filtering
// Brown & Hwang (Wiley,1997)
// p. 248
if(!kalman_filter_param_check(x,Pinv,Q,Rinv,A,H,z))
{
cerr << "Kalman filter parameters inconsistent !" << endl;
return FALSE;
}
EST_FMatrix K,I,At,Ht,P;
int singularity;
int state_dim=x.length();
eye(I,state_dim);
transpose(A,At);
transpose(H,Ht);
cerr << "Compute P" << endl;
// update error covariance
// =======================
Pinv = Pinv + (Ht * Rinv * H);
if(!inverse(Pinv,P,singularity))
{
if(singularity != -1)
{
cerr << "P is singular !" << endl;
return FALSE;
}
cerr << "Matrix inversion failed for an unknown reason !" << endl;
return FALSE;
}
// compute gain
// ============
K = P * Ht * Rinv;
// update state
// ============
x = add(x, K * subtract(z,H*x));
// project ahead
// =============
x = A * x;
P = A * P * At + Q;
if(!inverse(P,Pinv,singularity))
{
if(singularity != -1)
{
cerr << "Pinv is singular !" << endl;
return FALSE;
}
cerr << "Matrix inversion failed for an unknown reason !" << endl;
return FALSE;
}
// try and remedy numerical errors
//symmetrize(P);
//cerr << "done" << endl;
return TRUE;
}
bool kalman_filter_param_check(EST_FVector &x,
EST_FMatrix &P,
EST_FMatrix &Q,
EST_FMatrix &R,
EST_FMatrix &A,
EST_FMatrix &H,
EST_FVector &z)
{
int state_dim=x.length();
int measurement_dim=z.length();
// sanity checks
if((state_dim <= 0) ||
(measurement_dim <= 0))
{
cerr << "No state or measurements !!" << endl;
return FALSE;
}
// dimensionality
// P is error covariance
if((P.num_rows() != state_dim) ||
(P.num_columns() != state_dim) )
{
cerr << "P, or Pinv, must be a symmetrical square matrix of the same dimension" << endl;
cerr << "as the state vector, x" << endl;
return FALSE;
}
// Q is process noise covariance
if((Q.num_rows() != state_dim) ||
(Q.num_columns() != state_dim) )
{
cerr << "Q must be a symmetrical square matrix of the same dimension" << endl;
cerr << "as the state vector, x" << endl;
return FALSE;
}
// R is measurement noise covariance
if((R.num_rows() != measurement_dim) ||
(R.num_columns() != measurement_dim) )
{
cerr << "R, or Rinv, must be a symmetrical square matrix of the same dimension" << endl;
cerr << "as the measurement vector, z" << endl;
return FALSE;
}
if((A.num_rows() != state_dim) ||
(A.num_columns() != state_dim) )
{
cerr << "A must be a square matrix of the same dimension" << endl;
cerr << "as the state vector, x" << endl;
return FALSE;
}
if((H.num_rows() != measurement_dim) ||
(H.num_columns() != state_dim) )
{
cerr << "H must have dimensions to fit z = Hx" << endl;
return FALSE;
}
return TRUE;
}
EST_FMatrix
Definition EST_FMatrix.h:56
EST_FVector
Definition EST_FMatrix.h:114
EST_TMatrix::num_columns
int num_columns() const
return number of columns
Definition EST_TMatrix.h:181
EST_TMatrix::num_rows
int num_rows() const
return number of rows
Definition EST_TMatrix.h:179
EST_TVector::length
INLINE int length() const
number of items in vector.
Definition EST_TVector.h:252