Kalman滤波算法的C++实现
头文件：
/* * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), [email protected] * * This program is free software; you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the * Free Software Foundation, either version 2 or any later version. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * This program is distributed in the hope that it will be useful, but WITHOUT * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for * more details. A copy of the GNU General Public License is available at: * http://www.fsf.org/licensing/licenses */ /***************************************************************************** * kalman.h * * Kalman Filter. * * The Kalman filter is an efficient recursive filter that estimates the * internal state of a linear dynamic system from a series of noisy * measurements. In most applications, the internal state is much larger * (more degrees of freedom) than the few "observable" parameters which are * measured. However, by combining a series of measurements, the Kalman * filter can estimate the entire internal state. * * A wide variety of Kalman filters have now been developed, from Kalman's * original formulation, now called the simple Kalman filter, the Kalman-Bucy * filter, Schmidt's extended filter, the information filter, and a variety * of square-root filters that were developed by Bierman, Thornton and so on. * * Zhang Ming, 2010-10, Xi'an Jiaotong University. *****************************************************************************/ #ifndef KALMAN_H #define KALMAN_H #include <vector.h> #include <matrix.h> #include <inverse.h> namespace splab { template<typename Type> Vector<Type> kalman( const Matrix<Type>&, const Matrix<Type>&, const Matrix<Type>&, const Matrix<Type>&, const Vector<Type>&, const Vector<Type>&, const Vector<Type>& ); #include <kalman-impl.h> } // namespace splab #endif // KALMAN_H
实现文件：
/* * Copyright (c) 2008-2011 Zhang Ming (M. Zhang), [email protected] * * This program is free software; you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the * Free Software Foundation, either version 2 or any later version. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * This program is distributed in the hope that it will be useful, but WITHOUT * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for * more details. A copy of the GNU General Public License is available at: * http://www.fsf.org/licensing/licenses */ /***************************************************************************** * kalman-impl.h * * Implementation for Kalman Filter. * * Zhang Ming, 2010-10, Xi'an Jiaotong University. *****************************************************************************/ /** * The simple Kalman filter for one step. * A ---> system matrix defining linear dynamic system * C ---> measurement matrix defining relationship between system's state * and measurements * Q ---> covariance matrix of process noise in system state dynamics * R ---> covariance matrix of measurements uncertainty * y ---> measurements at time t * xPrev ---> previous estimated state vector of the linear dynamic system * initDiagV ---> diagonal vector for initializing the covariance matrix of * state estimation uncertainty */ template <typename Type> Vector<Type> kalman( const Matrix<Type> &A, const Matrix<Type> &C, const Matrix<Type> &Q, const Matrix<Type> &R, const Vector<Type> &xPrev, const Vector<Type> &y, const Vector<Type> &initDiagV ) { int N = xPrev.size(); // covariance matrix of state estimation uncertainty static Matrix<Type> V = diag(initDiagV); // previoused state vector Vector<Type> xPred = A * xPrev; // inovation Vector<Type> alpha = y - C * xPred; Matrix<Type> CTran = trT( C ); Matrix<Type> VPred = A*V*trT(A) + Q; // Kalman gain matrix Matrix<Type> KGain = VPred*CTran * inv(C*VPred*CTran+R); V = ( eye(N,Type(1.0)) - KGain*C ) * VPred; // return the estimation of the state vector return xPred + KGain * alpha; }
测试代码：
/*****************************************************************************
* kalman.h
*
* Kalman filter testing.
*
* Zhang Ming, 2010-10, Xi'an Jiaotong University.
*****************************************************************************/
#define BOUNDS_CHECK
#include <iostream>
#include <kalman.h>
using namespace std;
using namespace splab;
typedef double Type;
const int N = 2;
const int M = 2;
const int T = 20;
int main()
{
Matrix<Type> A(N,N), C(M,N), Q(N,N), R(M,M);
A = eye( N, Type(1.0) ); C = eye( N, Type(1.0) );
Q = eye( N, Type(1.0) ); R = eye( N, Type(2.0) );
Vector<Type> x(N,Type(1.0)), y(M), ytInit(M);
ytInit[0] = Type(0.5); ytInit[1] = Type(2.0);
Matrix<Type> yt(M,T);
for( int t=0; t<T; ++t )
yt.setColumn( ytInit, t );
Vector<Type> intV( N, Type(10.0) );
for( int t=0; t<T; ++t )
{
y = yt.getColumn(t);
x = kalman( A, C, Q, R, x, y, intV );
cout << "Estimation of xt at the " << t << "th iteratin: " << x << endl;
}
cout << "The theoretical xt should converge to: " << ytInit << endl;
return 0;
}
运行结果：
Estimation of xt at the 0th iteratin: size: 2 by 1 0.576923 1.84615 Estimation of xt at the 1th iteratin: size: 2 by 1 0.532787 1.93443 Estimation of xt at the 2th iteratin: size: 2 by 1 0.51581 1.96838 Estimation of xt at the 3th iteratin: size: 2 by 1 0.507835 1.98433 Estimation of xt at the 4th iteratin: size: 2 by 1 0.503909 1.99218 Estimation of xt at the 5th iteratin: size: 2 by 1 0.501953 1.99609 Estimation of xt at the 6th iteratin: size: 2 by 1 0.500977 1.99805 Estimation of xt at the 7th iteratin: size: 2 by 1 0.500488 1.99902 Estimation of xt at the 8th iteratin: size: 2 by 1 0.500244 1.99951 Estimation of xt at the 9th iteratin: size: 2 by 1 0.500122 1.99976 Estimation of xt at the 10th iteratin: size: 2 by 1 0.500061 1.99988 Estimation of xt at the 11th iteratin: size: 2 by 1 0.500031 1.99994 Estimation of xt at the 12th iteratin: size: 2 by 1 0.500015 1.99997 Estimation of xt at the 13th iteratin: size: 2 by 1 0.500008 1.99998 Estimation of xt at the 14th iteratin: size: 2 by 1 0.500004 1.99999 Estimation of xt at the 15th iteratin: size: 2 by 1 0.500002 2 Estimation of xt at the 16th iteratin: size: 2 by 1 0.500001 2 Estimation of xt at the 17th iteratin: size: 2 by 1 0.5 2 Estimation of xt at the 18th iteratin: size: 2 by 1 0.5 2 Estimation of xt at the 19th iteratin: size: 2 by 1 0.5 2 The theoretical xt should converge to: size: 2 by 1 0.5 2 Process returned 0 (0x0) execution time : 0.125 s Press any key to continue.