wemosでeigen その3

概要

wemosでeigenやってみた。
カルマンフィルターしてみた。

結果

サンプルコード

# include <Eigen.h>
# include <Eigen/LU>
# include <Eigen/Dense>
# include <math.h>
# include "kalman_filter.h"

# define PI					3.14159265

using namespace Eigen;
int i;
KalmanFilter kf;
MatrixXd R_laser_;
MatrixXd H_laser_;
MatrixXd Hj_;

void setup()
{
	Serial.begin(9600);
	Serial.println("start");
	kf = KalmanFilter();
	R_laser_ = MatrixXd(2, 2);
	H_laser_ = MatrixXd(2, 4);
	Hj_ = MatrixXd(3, 4);
	R_laser_ << 0.0225, 0, 0, 0.0225;
	Hj_ << 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 1, 1;
	kf.F_ = MatrixXd(4, 4);
	kf.F_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
	H_laser_ << 1, 0, 0, 0, 0, 1, 0, 0;
	kf.x_ = VectorXd(4);
	kf.x_ << 1, 1, 1, 1;
	kf.P_ = MatrixXd(4, 4);
	kf.P_ << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
	kf.H_ = H_laser_;
	kf.R_ = R_laser_;
	double dt = 0.01;
	float noise_ax = 9.0;
	float noise_ay = 9.0;
	double dt2 = dt * dt;
	double dt3 = dt2 * dt;
	double dt4 = dt2 * dt2;
	kf.Q_ = MatrixXd(4, 4);
	kf.Q_ << dt4 / 4 * noise_ax, 0, dt3 / 2 * noise_ax, 0, 0, dt4 / 4 * noise_ay, 0, dt3 / 2 * noise_ay, dt3 / 2 * noise_ax, 0, dt2 * noise_ax, 0, 0, dt3 / 2 * noise_ay, 0, dt2 * noise_ay;
	kf.F_(0, 2) = dt;
	kf.F_(1, 3) = dt;
}
void loop()
{
	i++;
	float d = sin(2 * PI * 50 * i / 20000) + 0.3 * sin(2 * PI * 4010 * i / 20000);
	Serial.print(d);
	VectorXd x = VectorXd(2);
	x << d, d;
	kf.Update(x);
	kf.Predict();
	double p_x = kf.x_(0);
	double p_y = kf.x_(1);
	double v1 = kf.x_(2);
	double v2 = kf.x_(3);
	Serial.print(",");
	Serial.println(p_x);
	delay(300);
}

以上。