std::shared_ptr<IBaseExprModel> CSystemControlModel::CopySelected() const
{
std::shared_ptr<CSystemControlModel> systemModel( new CSystemControlModel( rect, parent ) );
for ( auto child : children ) {
std::shared_ptr<IBaseExprModel> childModel = child->CopySelected();
childModel->SetParent(systemModel);
systemModel->children.push_back(childModel);
}
return systemModel;
}
//---------------------------------------------------------------------------------------------------------------------
void UnscentedKalmanFilter::forecastStep(const double _incT){
_incT; // 666 use system equation.
// Propagate sigma points through the nonlinear process model.
vector<MatrixXd> xfkPoints;
for(unsigned i = 0; i < mSigmaPoints.size(); i++) {
xfkPoints.push_back(systemModel(mSigmaPoints.at(i).first));
}
mXfk.setZero(mXfk.rows(), mXfk.cols()); // = MatrixXd::Zero(mXfk.rows(), mXfk.cols());
for (unsigned i = 0; i < mSigmaPoints.size(); i++) {
mXfk += xfkPoints.at(i) * mSigmaPoints.at(i).second;
}
mPk = mQk;
for (unsigned i = 0; i < mSigmaPoints.size(); i++) {
mPk += (xfkPoints.at(i) - mXfk)*(xfkPoints.at(i) - mXfk).transpose()*mSigmaPoints.at(i).second;
}
//Propagate sigma points through the nonlinear observation model.
vector<MatrixXd> zkPoints;
for (unsigned i = 0; i < mSigmaPoints.size(); i++) {
zkPoints.push_back(observerModel(mSigmaPoints.at(i).first));
}
mZk.setZero(mZk.rows(), mZk.cols()); //= MatrixXd::Zero(mZk.rows(), mZk.cols());
for (unsigned i = 0; i < mSigmaPoints.size(); i++) {
mZk += zkPoints.at(i) * mSigmaPoints.at(i).second;
}
mCovObs = mRk;
mCrossCov.setZero(mCrossCov.rows(), mCrossCov.cols()); // = MatrixXd::Zero(mCrossCov.rows(), mCrossCov.cols());
for (unsigned i = 0; i < mSigmaPoints.size(); i++) {
mCovObs = (zkPoints.at(i) - mZk)*(zkPoints.at(i) - mZk).transpose()*mSigmaPoints.at(i).second;;
mCrossCov = (xfkPoints.at(i) - mXfk)*(zkPoints.at(i) - mZk).transpose()*mSigmaPoints.at(i).second;;
}
mKk = mCrossCov*mCovObs;
}
