#include <StateEstimator/IteratedExtendedKalman.hh>
#include <Base/Math/Random.hh>
#include <fstream>
using namespace std;
using namespace BIAS;
public:
}
{
prediction[0] = state[0] + control[0]*state[1];
prediction[1] = state[1];
}
{
transJacobian[0][0] = 1;
transJacobian[0][1] = control[0];
transJacobian[1][0] = 0;
transJacobian[1][1] = 1;
transErrorJacobian[0][0] = 1;
transErrorJacobian[0][1] = 0;
transErrorJacobian[1][0] = 0;
transErrorJacobian[1][1] = 1;
}
const unsigned int index=1)
{
switch(index) {
case 1:
measurePred[0] = state[0]*state[0]*state[0];
break;
case 2:
measurePred[0] = state[1]*state[1]*state[1]+10*state[1];
break;
default:
BIASERR("Unknown measurement called!");
BIASABORT;
}
}
const unsigned int index=1)
{
switch(index) {
case 1:
measJacobian[0][0] = 3*state[0];
measJacobian[0][1] = 0;
measErrorJacobian[0][0] = 1;
break;
case 2:
measJacobian[0][0] = 0;
measJacobian[0][1] = 3*state[1]+10;
measErrorJacobian[0][0] = 1;
break;
default:
BIASERR("Unknown measurement called!");
BIASABORT;
}
}
};
int main(int argc, char *argv[]) {
if (argc != 5) {
cout << "Usage: " << argv[0]
<< " stateHistFile truePosFile measure1File measure2File!" << endl;
exit(-1);
}
OwnKF estimator;
unsigned int simLength = 500;
unsigned int rSeed = 0;
vector< BIAS::Vector<double> > measTrue1(simLength);
vector< BIAS::Vector<double> > measTrue2(simLength);
vector< BIAS::Vector<double> > measNoise1(simLength);
vector< BIAS::Vector<double> > measNoise2(simLength);
vector< BIAS::Vector<double> > stateHist(simLength);
vector< BIAS::Matrix<double> > covHist(simLength);
double sigmaM1 = 0.1;
double sigmaM2 = 0.01;
Q[0][0] = 0.001; Q[0][1] = 0.0; Q[1][0] = 0.0; Q[1][1] = 1e-5;
PI[0][0] = 0.001; PI[0][1] = 0.0; PI[1][0] = 0.0; PI[1][1] = 1;
for (unsigned int i=0; i<simLength; i++) {
posTrue[i] = sin(10*M_PI*double(i)/double(simLength));
posTrue[i] = 0.25*posTrue[i]*posTrue[i]+1.0;
velTrue[i] = 5.0*M_PI/double(simLength)
*sin(10*M_PI*double(i)/double(simLength))
*cos(10*M_PI*double(i)/double(simLength));
measTrue1[i].newsize(1);
measTrue1[i][0] = posTrue[i]*posTrue[i]*posTrue[i];
measTrue2[i].newsize(1);
measTrue2[i][0] = velTrue[i]*velTrue[i]*velTrue[i]+10*velTrue[i];
stateHist[i].newsize(2);
}
for (unsigned int i=0; i<simLength; i++) {
measNoise1[i][0] = measTrue1[i][0] +
rand.GetNormalDistributed (0.0, sigmaM1);
measNoise2[i].newsize(1);
measNoise2[i][0] = measTrue2[i][0] +
rand.GetNormalDistributed (0.0, sigmaM2);
}
control[0] = 1;
initial[0] = posTrue[0]; initial[1] = 0.0;
estimator.Init(initial, PI, Q);
for (unsigned int i=0; i<simLength; i++) {
estimator.Update(measNoise1[i], R_1, 1);
estimator.Update(measNoise2[i], R_2, 2);
estimator.GetPosterioriState(stateHist[i]);
estimator.GetPosterioriCovariance(covHist[i]);
estimator.Predict(control);
}
ofstream estFile(argv[1]);
ofstream gtFile(argv[2]);
ofstream meas1File(argv[3]);
ofstream meas2File(argv[4]);
for (unsigned int i=0; i < simLength; i++) {
estFile << stateHist[i][0] << " " << stateHist[i][1] << " "
<< sqrt(covHist[i][0][0]) << sqrt(covHist[i][1][1]) << endl;
gtFile << posTrue[i] << " " << velTrue[i] << endl;
meas1File << measNoise1[i][0] << endl;
meas2File << measNoise2[i][0] << endl;
}
estFile.close();
gtFile.close();
meas1File.close();
meas2File.close();
exit(0);
}