wemosでeigen その3

2214 ワード
Eigen カルマンフィルター Wemos Eigen テキストリンク

概要

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);
}

以上。