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_;
}
KalmanFilterBase::Amatrix// ExtendedKalmanFilter<n,m,p>::Amatrix does not work
ExtendedKalmanFilter::getAMatrixFD(const Vector
&dx)
{
TimeIndex k=this->x_.getTime();
opt.a_.resize(nt_,nt_);
opt.xbar_=prediction_(k+1);
opt.x_=this->x_();
opt.dx_.resize(nt_);
if (p_>0)
{
if (directInputStateProcessFeedthrough_)
opt.u_=this->u_[k];
else
opt.u_=inputVectorZero();
}
for (unsigned i=0;i<nt_;++i)
{
opt.dx_.setZero();
opt.dx_[i]=dx[i];
sum_(this->x_(),opt.dx_,opt.x_);
opt.xp_=f_->stateDynamics(opt.x_,opt.u_,k);
difference_(opt.xp_,opt.xbar_,opt.dx_);
opt.dx_/=dx[i];
opt.a_.col(i)=opt.dx_;
}
return opt.a_;
}
Vector KalmanFilterBase::predictSensor_(TimeIndex k)
{
oc_.xbar = prediction_(k);
return predictedMeasurement_=simulateSensor_(oc_.xbar,k);
}
/*inline*/ void KalmanFilterBase::updatePrediction()
{
oc_.xbar = prediction_(this->x_.getTime()+1);
}
/*inline*/ Vector KalmanFilterBase::updateStatePrediction()
{
return oc_.xbar = prediction_(this->x_.getTime()+1);
}