predict.cc

2007年机器人足球世界杯3D仿真组亚军于07年底的源程序。该队还于2008年获得伊朗公开赛的亚军。

#include "normaldistribution3d.ih"

void NormalDistribution3D::predict(rf<IDistribution3D> const &movementModel)
{
  Vector3D movMu = movementModel->getMu();
  Matrix9D movSigma = movementModel->getSigma();

  _debugLevel2("Before prediction: " << d_mu[0] << "\t" << d_mu[1] << "\t" << d_mu[2]);
  _debugLevel2(movMu[0] << "\t" << movMu[1] << "\t" << movMu[2]);
  // x_k^- = A x_k-1 + B u_k-1
  // where u is the movement model
  // All movement is accumulated in the movement model. so there is no extra movement: A = I and no translation is needed: B = 1, so:
  // x_k^- = x_k-1 + u_k-1
  d_mu += movMu;
  
  // S_k^- = A P_k-1^+ A^T + Q_k-1 = P_k-1^+ + Q_k-1
  d_sigma = d_sigma + movSigma;
  
  _debugLevel2("Prediction: " << d_mu[0] << "\t" << d_mu[1] << "\t" << d_mu[2]);
}