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_;
}
//--------------------------------------------------------
// apply feedback charges to atoms
//--------------------------------------------------------
void ChargeRegulatorMethodFeedback::apply_feedback_charges()
{
double * q = lammpsInterface_->atom_charge();
// calculate error in potential on the control nodes
const DENS_MAT & phiField = (atc_->field(ELECTRIC_POTENTIAL)).quantity();
DENS_MAT dphi(nControlNodes_,1);
int i = 0;
set<int>::const_iterator itr;
for (itr = controlNodes_.begin(); itr != controlNodes_.end(); itr++) {
dphi(i++,0) = targetPhi_ - phiField(*itr,0);
}
// construct the atomic charges consistent with the correction
DENS_MAT dq = NTinvNNTinvG_*dphi;
i = 0;
for (itr = influenceAtomsIds_.begin(); itr != influenceAtomsIds_.end(); itr++) {
sum_(0) += dq(i,0);
q[*itr] += dq(i++,0);
}
(interscaleManager_->fundamental_atom_quantity(LammpsInterface::ATOM_CHARGE))->force_reset();
(interscaleManager_->fundamental_atom_quantity(LammpsInterface::ATOM_CHARGE, GHOST))->force_reset();
}
static int sum64f( const double* src, const uchar* mask, double* dst, int len, int cn )
{ CV_INSTRUMENT_REGION(); return sum_(src, mask, dst, len, cn); }
static int sum16s( const short* src, const uchar* mask, int* dst, int len, int cn )
{ CV_INSTRUMENT_REGION(); return sum_(src, mask, dst, len, cn); }
static int sum64f( const double* src, const uchar* mask, double* dst, int len, int cn )
{ return sum_(src, mask, dst, len, cn); }
static int sum16s( const short* src, const uchar* mask, int* dst, int len, int cn )
{ return sum_(src, mask, dst, len, cn); }
ObserverBase::StateVector KalmanFilterBase::oneStepEstimation_()
{
TimeIndex k=this->x_.getTime();
BOOST_ASSERT(this->y_.size()> 0 && this->y_.checkIndex(k+1) && "ERROR: The measurement vector is not set");
BOOST_ASSERT(checkAmatrix(a_) && "ERROR: The Matrix A is not initialized" );
BOOST_ASSERT(checkCmatrix(c_) && "ERROR: The Matrix C is not initialized");
BOOST_ASSERT(checkQmatrix(q_) && "ERROR: The Matrix Q is not initialized");
BOOST_ASSERT(checkRmatrix(r_) && "ERROR: The Matrix R is not initialized");
BOOST_ASSERT(checkPmatrix(pr_) && "ERROR: The Matrix P is not initialized");
//prediction
updateStateAndMeasurementPrediction();// runs also updatePrediction_();
oc_.pbar.noalias()=q_ +a_*(pr_*a_.transpose());
//innovation Measurements
oc_.inoMeas.noalias() = this->y_[k+1] - predictedMeasurement_;
oc_.inoMeasCov.noalias() = r_ + c_ * (oc_.pbar * c_.transpose());
unsigned & measurementSize =m_;
//inversing innovation measurement covariance matrix
oc_.inoMeasCovLLT.compute(oc_.inoMeasCov);
oc_.inoMeasCovInverse.resize(measurementSize,measurementSize);
oc_.inoMeasCovInverse.setIdentity();
oc_.inoMeasCovLLT.matrixL().solveInPlace(oc_.inoMeasCovInverse);
oc_.inoMeasCovLLT.matrixL().transpose().solveInPlace(oc_.inoMeasCovInverse);
//innovation
oc_.kGain.noalias() = oc_.pbar * (c_.transpose() * oc_.inoMeasCovInverse);
innovation_.noalias() = oc_.kGain*oc_.inoMeas;
//update
sum_(oc_.xbar,innovation_,oc_.xhat);
#ifdef VERBOUS_KALMANFILTER
Eigen::IOFormat CleanFmt(2, 0, " ", "\n", "", "");
std::cout <<"A" <<std::endl<< a_.format(CleanFmt)<<std::endl;
std::cout <<"C" <<std::endl<< c_.format(CleanFmt)<<std::endl;
std::cout <<"P" <<std::endl<< pr_.format(CleanFmt)<<std::endl;
std::cout <<"K" <<std::endl<< oc_.kGain.format(CleanFmt)<<std::endl;
std::cout <<"Xbar" <<std::endl<< oc_.xbar.transpose().format(CleanFmt)<<std::endl;
std::cout <<"inoMeasCov" <<std::endl<< oc_.inoMeasCov.format(CleanFmt)<<std::endl;
std::cout <<"oc_.pbar" <<std::endl<< (oc_.pbar).format(CleanFmt)<<std::endl;
std::cout <<"c_ * (oc_.pbar * c_.transpose())" <<std::endl<< ( c_ * (oc_.pbar * c_.transpose())).format(CleanFmt)<<std::endl;
std::cout <<"inoMeasCovInverse" <<std::endl<< oc_.inoMeasCovInverse.format(CleanFmt)<<std::endl;
std::cout <<"predictedMeasurement " <<std::endl<< predictedMeasurement_.transpose().format(CleanFmt)<<std::endl;
std::cout <<"inoMeas" <<std::endl<< oc_.inoMeas.transpose().format(CleanFmt)<<std::endl;
std::cout <<"inovation_" <<std::endl<< inovation_.transpose().format(CleanFmt)<<std::endl;
std::cout <<"Xhat" <<std::endl<< oc_.xhat.transpose().format(CleanFmt)<<std::endl;
#endif // VERBOUS_KALMANFILTER
this->x_.set(oc_.xhat,k+1);
pr_.noalias() = -oc_.kGain*c_;
pr_.diagonal().array()+=1;
pr_ *= oc_.pbar;
// simmetrize the pr_ matrix
pr_=(pr_+pr_.transpose())*0.5;
return oc_.xhat;
}
