Results

main.cpp

		c
/*  Copyright (C) 2016  Felipe Bombardelli <felipebombardelli@gmail.com>
 *
 *  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 3 of the License, or
 *  (at your option) any later version.
 *
 *  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.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */


/*=====================================- Header -========================================*/


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>


using namespace std;

/*---------------------------------------------------------------------------------------*/



/*================================- Kalman Filter -======================================*/

#define M_SIZE 2

typedef Eigen::Matrix<double, 2, 2> Matf;
typedef Eigen::Matrix<double, 2, 1> Vetf;

class KalmanFilter {
  // It changes to each iteration
	Vetf  x, x_r; 
	Matf  P;

  //Constants
	Vetf B;
	Matf H, Q, R, F, I;
	int size;

  private:
	Matf S, K;

  public:
	KalmanFilter(int size){
		this->size = size;
		
		srand(123);
		F << 1,1,
		      0,1;
		B << 0.5, 1;

		H << 1, 0,
		     0, 1;
		P << 0, 0,
		     0, 0;

		I << 1, 0,
		     0, 1;


		Q << 0.2, 0,
		     0,  0.2;


		R << 30, 0,
		     0, 2;

		x << 0,0;
	}

	Vetf simulate(double u){
		// Create the noise
		Vetf r;
		
		int sinal = 1;
		if ( (rand()%512) > 256 )
			sinal = -1;
		
		double r1 = sinal * (rand()/(float)RAND_MAX)*30;
		double r2 = sinal * (rand()/(float)RAND_MAX)*1;
		r << r1, r2;
		cout << "ruido " << r1 << " " << r2 << endl;
	
		x_r = F * x_r + B * u;
		cout << "Real:\n" << x_r << endl;
	
		// Create the signal with noise
		return x_r + r;
	}


	void predict(double u){
		this->x = F * x + B * u;
		this->P = F * P * F.transpose() + Q;
		cout << "X_1:\n" << this->x << endl;
		//cout << "P1:\n" << P << endl;
	}

	void update(Vetf& z){
		// Update the value
		Vetf y = z - this->H * this->x;
		//cout << "Y: \n" << y << endl;
		
		S = H * P * H.transpose() + R;
		//cout << "S:\n" << S << endl;
		
		K = P * H.transpose() * S.inverse();
		cout << "K:\n" << K << endl;
		
		P = (I - K*H) * P;
		//cout << "P2:\n" << P << endl;
		
		x = x + K * y;
		cout << "X_2: \n" << x << endl;
	}

};

/*---------------------------------------------------------------------------------------*/


/*======================================- Main -=========================================*/

int main(int argc, char** argv){
	KalmanFilter kf(2);
	for (int i=0; i<20; i++){
		Vetf z = kf.simulate(1.0);
		cout << "Z:\n" << z << endl;
		kf.predict(1.0);
		kf.update(z);
		cout << "fim\n\n";
	}

	return 0;
}
/*---------------------------------------------------------------------------------------*/