KalmanFilterBase::Cmatrix ExtendedKalmanFilter::getCMatrixFD(const Vector &dx) { TimeIndex k=this->x_.getTime(); opt.c_.resize(m_,nt_); opt.xbar_=prediction_(k+1); opt.xp_ = opt.xbar_; opt.y_=predictSensor_(k+1); opt.dx_.resize(nt_); for (unsigned i=0;i<nt_;++i) { opt.dx_.setZero(); opt.dx_[i]=dx[i]; sum_(opt.xbar_,opt.dx_,opt.xp_); opt.yp_=simulateSensor_(opt.xp_, k+1); opt.yp_-=opt.y_; opt.yp_/=dx[i]; opt.c_.col(i)=opt.yp_; } return opt.c_; }
Vector KalmanFilterBase::getSimulatedMeasurement(unsigned k) { return simulateSensor_(getEstimatedState(k),k); }
/*inline*/ void KalmanFilterBase::updatePredictedMeasurement() { updatePrediction(); predictedMeasurement_=simulateSensor_(oc_.xbar,this->x_.getTime()+1); }
Vector KalmanFilterBase::predictSensor_(TimeIndex k) { oc_.xbar = prediction_(k); return predictedMeasurement_=simulateSensor_(oc_.xbar,k); }