原理

就這樣不停的移動(dòng)更新，最終小車的位置就會(huì)越來越準(zhǔn)確。

一維模型下的Kalman公式：

參考代碼：

#include 
#include 
#include 


using namespace std;


double new_mean, new_var;


tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
  new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
  new_var = 1 / (1 / var1 + 1 / var2);
  return make_tuple(new_mean, new_var);
}


tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
  new_mean = mean1 + mean2;
  new_var = var1 + var2;
  return make_tuple(new_mean, new_var);
}


int main()
{
  //Measurements and measurement variance
  double measurements[5] = { 5, 6, 7, 9, 10 };
  double measurement_sig = 4;
  
  //Motions and motion variance
  double motion[5] = { 1, 1, 2, 1, 1 };
  double motion_sig = 2;
  
  //Initial state
  double mu = 0;
  double sig = 1000;


  for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
    tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
    printf("update: [%f, %f]
", mu, sig);
    tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
    printf("predict: [%f, %f]
", mu, sig);
  }


  return 0;
}

審核編輯 ：李倩