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; }