example.cpp

Go to the documentation of this file.

#include <cstdlib>
#include <iostream>
#include <fstream>

#include <cmath>

// Example of Extented Kalman Filter
#include "plane.h"

using namespace std;
using namespace Kalman;

// Simple uniform distribution of zero mean and unit variance
float uniform(void)
{
   return((((float)rand())/(RAND_MAX-1) - 0.5f)* 3.464101615138f);
}

// Simple approximation of normal dist. by the sum of uniform dist.
float normal()
{
  int n = 6;
  int i;
  float temp = 0.0;

  for(i = 0; i < n; i++) 
    temp += uniform();
  temp /= sqrt((float)n);
  return temp;
}


int main() {

        ifstream dataInput;
        ofstream dataOutput;
        std::string tmpStr;

        const unsigned NTRY = 500;
        const unsigned n = 4;   //nb states
        const unsigned m = 2;   //nb measures

        // Matlab format
        selectKVectorContext(createKVectorContext(" ", "[ ", " ];", 4));
        selectKMatrixContext(createKMatrixContext(" ", " ;\n  ", "[ ", " ];", 4));

        cPlaneEKF filter;

        static const double _P0[] = {100.0*100.0, 0.0, 0.0, 0.0,
                                                                 0.0, 10.0*10.0, 0.0, 0.0,
                                                                 0.0, 0.0, 25.0*25.0, 0.0,
                                                                 0.0, 0.0, 0.0, 10.0*10.0}; 

        Vector x(n);
        Matrix P0(n, n, _P0);

        Vector F(NTRY);
        Matrix Measure(m,NTRY);

        dataInput.open("../Matlab/data.m",ifstream::in);
        dataOutput.open("../Matlab/trajectory_udu_load.m", ofstream::out | ofstream::trunc);

        if (dataInput.fail())
        {
                cout<<"Unable to open input file!"<<endl;
                return 0;
        }

        if (dataOutput.fail())
        {
                cout<<"Unable to open output file!"<<endl;
                return 0;
        }

        cout<<"Loading inputs and measures from file <data.m>."<<endl;

        //Read the inputs vector. This vector have been generated by the Matlab script <generation.m>
        dataInput>>tmpStr;
        dataInput>>tmpStr;
        dataInput>>F;

        if (dataInput.fail())
        {
                cout<<"IO error!"<<endl;
                return 0;
        }

        //Output as a Matlab colonn vector.  This will be used by the Matlab script <result.m>
        selectKVectorContext(createKVectorContext(";", "[ ", " ];", 4));


        //Read the measures matrix. This matrix have been generated by the Matlab script <generation.m>
        dataInput>>tmpStr;
        dataInput>>tmpStr;
        dataInput>>tmpStr;
        dataInput>>Measure;

        if (dataInput.fail())
        {
                cout<<"IO error!"<<endl;
                return 0;
        }

        unsigned i=1, j=1;
        Vector z(m);

        //Initiale estimate
        cout<<"angle: "<<Measure(1,1)<<"rayon: "<<Measure(2,1)<<endl;
        x(1) = cos(Measure(1,1))*Measure(2,1);
        x(2) = 60;
        x(3) = sin(Measure(1,1))*Measure(2,1);
        x(4) = 0;

        filter.init(x, P0);

        cout << "xp(" << ":," << i<<") = " << filter.getX()<<endl;

        dataOutput<<"trajectory_udu(" << ":," << i <<") = " << filter.getX()<<endl;

        for (i = 2; i <= NTRY; ++i) 
        {
                // filter
                for(j = 1; j <= m; j++)
                        z(j) = Measure(j,i);

                Vector u(1, F(i));

                filter.step(u, z);

                cout << "xp(" << ":," << i<<") = " << filter.getX()<<endl;
                dataOutput<<"trajectory_udu(" << ":," << i<<") = " << filter.getX()<<endl;
        }


        dataOutput.close();
        dataInput.close();

        return EXIT_SUCCESS;
}