Example using a very simple filter for estimating a constant value. Therefore the state- transition-function is the identity (nothing happens during timestep) and the measurement-function is a direct observation of the state (identity).
#include <Base/Math/Random.hh>
#include <StateEstimator/ExtendedKalman.hh>
#ifdef WIN32
#include <Base/Common/W32Compat.hh>
#else //Linux
#include <sys/time.h>
#endif
namespace BIAS {
class MyKalman : public ExtendedKalman {
public:
MyKalman(){};
virtual ~MyKalman(){};
virtual void MeasurementFunction(const Vector<double>& state,
Vector<double>& measurePred)
{
measurePred = state;
}
virtual void StateTransition(const Vector<double>& oldState,
const Vector<double>& control,
Vector<double>& newState)
{
newState = oldState;
}
};
}
using namespace std;
using namespace BIAS;
int main(int argc, char ** argv )
{
if (argc < 2) {
cout << "Give number of iterations!" << endl;
exit(0);
}
double mean1 = 20.0;
double mean2 = 30.5;
MyKalman filter;
int maxIter = atoi(argv[1]);
zero.SetZero();
cov.SetZero();
cov[0][0] = 4.0;
cov[1][1] = 16.0;
id.SetIdentity();
filter.SetInitial(state, 100.0*cov);
filter.SetMeasureDerive(id, id);
filter.SetStateDerive(id, zero);
filter.SetMeasureCov(cov);
filter.SetProcessCov(zero);
double sum1 = 0, sum2 = 0;
for (int i=0; i<maxIter; i++) {
observation[0] =
sum1 += observation[0];
observation[1] =
sum2 += observation[1];
filter.Update(observation);
filter.Predict(ctrlparam);
}
filter.GetState(state);
cout << "should be 1: " << mean1 << " 2: " << mean2
<< endl << flush;
cout << "kalman state 1: " << state[0] << " 2: " << state[1]
<< endl << flush;
cout << "avarage: 1: " << sum1/double(maxIter) << " 2: "
<< sum2/double(maxIter) << endl << flush;
return 0;
}