kalman.cc

#include <iostream>
#include <fstream>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/LU>

int main(){
	constexpr int deg = 2;
	constexpr double omega = 2.0*M_PI*2.0;
	constexpr double T = 0.01;

	Eigen::Matrix2d A;
	A << cos(omega*T) , sin(omega*T)/omega , -omega*sin(omega*T) , cos(omega*T);
	Eigen::Vector2d b;
	b << (1.0-cos(omega*T))/(omega*omega) , sin(omega*T)/omega ;
	Eigen::RowVector2d c;
	c << 0,1;

	Eigen::Matrix2d Q;
	Q << 0,0,0,0;
	double R=1.e-3;//観測誤差の共分散行列(実数)

	std::cerr<<"A:"<<A<<std::endl;
	std::cerr<<"b:"<<b<<std::endl;
	std::cerr<<"c:"<<c<<std::endl;

	constexpr int step = 1000;

	std::vector<Eigen::Vector2d> x(step),xhat(step),xhatbar(step);
	std::vector<double> y(step),ytilde(step);
	std::vector<Eigen::Matrix2d> Sigma(step),Sigmabar(step);
	std::vector<double> S(step);
	std::vector<Eigen::Vector2d> K(step);
	std::vector<double> u(step);
	
	for(int i=0;i<step;++i){
		//u[i] = 5.0*sin(2.0*M_PI*i*T);
		u[i] = 5.0;
	}

	constexpr double sd = 1.0;
	std::random_device rnd;
	std::mt19937 mt(rnd());
	std::normal_distribution<> norm(0,sd);

	for(int i=1;i<step;++i){
		x[i] = A*x[i-1]+b*u[i-1];
		double ytrue = c*x[i];
		y[i] = ytrue+norm(mt);

		//prediction
		xhatbar[i] = A*xhat[i-1] + b*u[i-1];
		Sigma[i] = Q+A*Sigma[i-1]*A.transpose();

		//update
		ytilde[i] = y[i] - c*xhat[i];
		S[i] = c*Sigmabar[i]*c.transpose() + R;
		K[i] = Sigmabar[i]*c.transpose()*(1.0/S[i]);
		xhat[i] = xhatbar[i] + K[i]*ytilde[i];
		Sigma[i] = Sigmabar[i] - K[i]*c*Sigmabar[i];

		std::cout<<T*i<<" "<<ytrue<<" "<<y[i]<<" "<<c*xhat[i]<<" "<<(c*xhat[i]-ytrue)<<std::endl;

	}
	return 0;
}