一次元の値を予測する( ? )カルマンフィルタの実装．http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf をコードに落としたら上手く動いた．http://scipy.github.io/old-wiki/pages/Cookbook/KalmanFiltering のC++版に相当するはず．

simple_kalman_filter.cc

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <string>
#include <fstream>


int main(int argc, char* argv[]) {
  if(argc != 2) {
    std::cout << "usage : this.out [output csv name]" << std::endl;
    return -1;
  }

  /* pre-process ---------------------------------------- */
  const double d_true_value = 10.0;
  const double d_variance = 1.0;
  std::random_device seed;
	std::mt19937 engine(seed());

	double d_mu = d_true_value;
  double d_sig = sqrt(d_variance);
	std::normal_distribution<> dist(d_mu, d_sig);
	const int N = 100;
	std::vector<double> vd_sample_list(N);

	for (int i=0; i<N; ++i) {
		vd_sample_list[i] = dist(engine);
    // std::cout << vd_sample_list[i] << std::endl;
	}
  /*  ---------------------------------------- pre-process */

  const double d_Q = 0.00000001; // == 0
  double d_R = d_variance;

  std::vector<double> vd_z = vd_sample_list;

  std::vector<double> vd_x_hat_minus(N, 0.0);
  std::vector<double> vd_P_minus(N, 0.0);
  std::vector<double> vd_x_hat(N, 0.0);
  std::vector<double> vd_P(N, 0.0);
  std::vector<double> vd_K(N, 0.0);

  vd_x_hat[0] = 0.0;
  vd_P[0] = 1.0;

  for(int k = 1; k < N; k++) {
    // Time Update Equations
    vd_x_hat_minus[k] = vd_x_hat[k-1];
    vd_P_minus[k] = vd_P[k-1] + d_Q;

    // Measurememnt Update Equations
    vd_K[k]  = vd_P_minus[k]/(vd_P_minus[k] + d_R);
    vd_x_hat[k] = vd_x_hat_minus[k] + vd_K[k]*(vd_z[k] - vd_x_hat_minus[k]);
    vd_P[k] = (1.0 - vd_K[k]) * vd_P_minus[k];
  }

  std::string s_csv = argv[1];
  std::ofstream fs(s_csv);

  for(int i = 0; i < vd_x_hat.size(); i ++) {
    fs << vd_x_hat[i] << ",";
  }

  fs.flush();
  fs.close();

  return 0;
}