void SimMathcalBongo::myintegrate() {
double h = 1.0 * mTimestep;
double h_2 = 0.5 * h;
Eigen::VectorXd x = mState;
Eigen::VectorXd u = mTorque;
// LOG(INFO) << "math.x = " << x.transpose();
// LOG(INFO) << "math.u = " << u.transpose();
Eigen::VectorXd k1 = deriv(x, u);
Eigen::VectorXd k2 = deriv(x + h_2 * k1, u);
Eigen::VectorXd k3 = deriv(x + h_2 * k2, u);
Eigen::VectorXd k4 = deriv(x + h * k3, u);
Eigen::VectorXd dx = (k1 + 2 * k2 + 2 * k3 + k4) / 6.0;
// LOG(INFO) << "dx = " << dx.transpose();
mState = x + h * dx;
for (int i = 0; i < mState.size(); i++) {
double x = mState(i);
if (x < -2.0 * PI) x = -2.0 * PI;
if (x > 2.0 * PI) x = 2.0 * PI;
mState(i) = x;
}
// mHistory.push_back(mState);
// // Hard coded constraint..
mState(3) = mState(2);
mState(4) = 0.5 * PI - mState(2);
mState(5) = -0.5 * PI - mState(2);
}
bool EkfSlam::getMap(Mat& map, Mat &cov)
{
if(mState.rows > 3) {
map = mState(Rect(0, 3, 1, mState.rows-3)).clone();
cov = Pmm.clone();
return true;
}
return false;
}
void EkfSlam::addFeature(Mat yi, int i)
{
printf("Discovered Landmark %d\n", i);
Mat s2 = s.clone();
s2.at<double>(0) *= s2.at<double>(0);
s2.at<double>(1) *= s2.at<double>(1);
Mat R = Mat::diag(s2);
Mat Gr = dg_dxb(mState(Rect(0,0,1,3)), yi);
Mat Gy = dg_dyi(mState(Rect(0,0,1,3)), yi);
Mat L = g(mState(Rect(0,0,1,3)), yi);
mState.push_back(L);
Mat Pll = Gr*Prr*Gr.t() + Gy*R*Gy.t();
Mat Plr = Gr*Prr;
Mat Plm;
if(!i) {
Pmr = Plr.clone();
Prm = Pmr.t();
Pmm = Pll.clone();
}
else {
Plm = Gr*Prm;
Pmr.push_back(Plr);
Prm = Pmr.t();
// append Pml to right of Pmm
Pmm = Pmm.t();
Pmm.push_back(Plm);
Pmm = Pmm.t();
// append Pll to right of Plm
Plm = Plm.t();
Pll = Pll.t();
Plm.push_back(Pll);
Plm = Plm.t();
// append [Plm|Pll] to bottom of Pmm
Pmm.push_back(Plm);
}
}
void QtPosSaver::SaveGeometry(QWidget *widget)
{
if(NULL != widget && !attachedWidgetName.isEmpty())
{
QString key = attachedWidgetName + "-geometry-" + widget->objectName();
Save(key, widget->saveGeometry());
key = attachedWidgetName + "-maximized-" + widget->objectName();
QByteArray mState(1, (char) widget->isMaximized());
Save(key, mState);
}
}
Mat EkfSlam::step(Mat u)
{
Mat observations;
Mat n = (Mat_<double>(2,1) << 0, 0);
Mat v = (Mat_<double>(2,1) << 0, 0);
Mat Y;
// motion prediction updates covariance and state vector
move(mState(Rect(0,0,1,3)), u, n);
// landmark correction
for(int i=0; i < (mState.rows-3)/2; i++) {
v.at<double>(0) = s.at<double>(0)*r->gaussian(.5);
v.at<double>(1) = s.at<double>(1)*r->gaussian(.5);
Mat yi = scan(i);
if(yi.rows) {
observations.push_back(yi);
yi+=v;
correct(scan(i), v, i);
}
}
// initialize any new landmarks discovered
for(int i=(mState.rows-3)/2;;i++) {
Mat f = scan(i);
if(f.rows == 2) {
addFeature(f,i);
observations.push_back(f);
}
else
break;
}
// return raw sensor data for simple navigation tasks
return observations;
}
int DruckerPrager::recvSelf(int commitTag, Channel &theChannel, FEM_ObjectBroker &theBroker)
{
int res = 0;
// receive data
static Vector data(45);
res = theChannel.recvVector(this->getDbTag(), commitTag, data);
if (res < 0) {
opserr << "WARNING: DruckerPrager::recvSelf - failed to receive vector from channel" << endln;
return -1;
}
// set member variables
this->setTag((int)data(0));
mKref = data(1);
mGref = data(2);
mK = data(3);
mG = data(4);
msigma_y = data(5);
mrho = data(6);
mrho_bar = data(7);
mKinf = data(8);
mKo = data(9);
mdelta1 = data(10);
mdelta2 = data(11);
mHard = data(12);
mtheta = data(13);
massDen = data(14);
mPatm = data(15);
mTo = data(16);
mHprime = data(17);
mAlpha1_n = data(18);
mAlpha2_n = data(19);
mElastFlag = (int)data(20);
mFlag = (int)data(21);
mEpsilon(0) = data(22);
mEpsilon(1) = data(23);
mEpsilon(2) = data(24);
mEpsilon(3) = data(25);
mEpsilon(4) = data(26);
mEpsilon(5) = data(27);
mEpsilon_n_p(0) = data(28);
mEpsilon_n_p(1) = data(29);
mEpsilon_n_p(2) = data(30);
mEpsilon_n_p(3) = data(31);
mEpsilon_n_p(4) = data(32);
mEpsilon_n_p(5) = data(33);
mBeta_n(0) = data(34);
mBeta_n(1) = data(35);
mBeta_n(2) = data(36);
mBeta_n(3) = data(37);
mBeta_n(4) = data(38);
mBeta_n(5) = data(39);
mState(0) = data(40);
mState(1) = data(41);
mState(2) = data(42);
mState(3) = data(43);
mState(4) = data(44);
mCe = mK*mIIvol + 2*mG*mIIdev;
mCep = mCe;
return 0;
}
int DruckerPrager::sendSelf(int commitTag, Channel &theChannel)
{
int res = 0;
// place data in a vector
static Vector data(45);
data(0) = this->getTag();
data(1) = mKref;
data(2) = mGref;
data(3) = mK;
data(4) = mG;
data(5) = msigma_y;
data(6) = mrho;
data(7) = mrho_bar;
data(8) = mKinf;
data(9) = mKo;
data(10) = mdelta1;
data(11) = mdelta2;
data(12) = mHard;
data(13) = mtheta;
data(14) = massDen;
data(15) = mPatm;
data(16) = mTo;
data(17) = mHprime;
data(18) = mAlpha1_n;
data(19) = mAlpha2_n;
data(20) = mElastFlag;
data(21) = mFlag;
data(22) = mEpsilon(0);
data(23) = mEpsilon(1);
data(24) = mEpsilon(2);
data(25) = mEpsilon(3);
data(26) = mEpsilon(4);
data(27) = mEpsilon(5);
data(28) = mEpsilon_n_p(0);
data(29) = mEpsilon_n_p(1);
data(30) = mEpsilon_n_p(2);
data(31) = mEpsilon_n_p(3);
data(32) = mEpsilon_n_p(4);
data(33) = mEpsilon_n_p(5);
data(34) = mBeta_n(0);
data(35) = mBeta_n(1);
data(36) = mBeta_n(2);
data(37) = mBeta_n(3);
data(38) = mBeta_n(4);
data(39) = mBeta_n(5);
data(40) = mState(0);
data(41) = mState(1);
data(42) = mState(2);
data(43) = mState(3);
data(44) = mState(4);
res = theChannel.sendVector(this->getDbTag(), commitTag, data);
if (res < 0) {
opserr << "WARNING: DruckerPrager::sendSelf - failed to send vector to channel" << endln;
return -1;
}
return 0;
}
//--------------------Plasticity-------------------------------------
//plasticity integration routine
void DruckerPrager:: plastic_integrator( )
{
bool okay; // boolean variable to ensure satisfaction of multisurface kuhn tucker conditions
double f1;
double f2;
double norm_eta;
double Invariant_1;
double Invariant_ep;
double norm_ep;
double norm_dev_ep;
Vector epsilon_e(6);
Vector s(6);
Vector eta(6);
Vector dev_ep(6);
Vector Jact(2);
double fTOL;
double gTOL;
fTOL = 0.0;
gTOL = -1.0e-10;
double NormCep;
double alpha1; // hardening parameter for DP surface
double alpha2; // hardening parameter for tension cut-off
Vector n(6); // normal to the yield surface in strain space
Vector R(2); // residual vector
Vector gamma(2); // vector of consistency parameters
Vector dgamma(2); // incremental vector of consistency parameters
Matrix g(2,2); // jacobian of the corner region (return map)
Matrix g_contra(2,2); // inverse of jacobian of the corner region
// set trial state:
// epsilon_n1_p_trial = ..._n1_p = ..._n_p
mEpsilon_n1_p = mEpsilon_n_p;
// alpha1_n+1_trial
mAlpha1_n1 = mAlpha1_n;
// alpha2_n+1_trial
mAlpha2_n1 = mAlpha2_n;
// beta_n+1_trial
mBeta_n1 = mBeta_n;
// epsilon_elastic = epsilon_n+1 - epsilon_n_p
epsilon_e = mEpsilon - mEpsilon_n1_p;
// trial stress
mSigma = mCe*epsilon_e;
// deviator stress tensor: s = 2G * IIdev * epsilon_e
//I1_trial
Invariant_1 = ( mSigma(0) + mSigma(1) + mSigma(2) );
// s_n+1_trial
s = mSigma - (Invariant_1/3.0)*mI1;
//eta_trial = s_n+1_trial - beta_n;
eta = s - mBeta_n;
// compute yield function value (contravariant norm)
norm_eta = sqrt(eta(0)*eta(0) + eta(1)*eta(1) + eta(2)*eta(2) + 2*(eta(3)*eta(3) + eta(4)*eta(4) + eta(5)*eta(5)));
// f1_n+1_trial
f1 = norm_eta + mrho*Invariant_1 - root23*Kiso(mAlpha1_n1);
// f2_n+1_trial
f2 = Invariant_1 - T(mAlpha2_n1);
// update elastic bulk and shear moduli
this->updateElasticParam();
// check trial state
int count = 1;
if ((f1<=fTOL) && (f2<=fTOL) || mElastFlag < 2) {
okay = true;
// trial state = elastic state - don't need to do any updates.
mCep = mCe;
count = 0;
// set state variables for recorders
Invariant_ep = mEpsilon_n1_p(0)+mEpsilon_n1_p(1)+mEpsilon_n1_p(2);
norm_ep = sqrt(mEpsilon_n1_p(0)*mEpsilon_n1_p(0) + mEpsilon_n1_p(1)*mEpsilon_n1_p(1) + mEpsilon_n1_p(2)*mEpsilon_n1_p(2)
+ 0.5*(mEpsilon_n1_p(3)*mEpsilon_n1_p(3) + mEpsilon_n1_p(4)*mEpsilon_n1_p(4) + mEpsilon_n1_p(5)*mEpsilon_n1_p(5)));
dev_ep = mEpsilon_n1_p - one3*Invariant_ep*mI1;
norm_dev_ep = sqrt(dev_ep(0)*dev_ep(0) + dev_ep(1)*dev_ep(1) + dev_ep(2)*dev_ep(2)
+ 0.5*(dev_ep(3)*dev_ep(3) + dev_ep(4)*dev_ep(4) + dev_ep(5)*dev_ep(5)));
mState(0) = Invariant_1;
mState(1) = norm_eta;
mState(2) = Invariant_ep;
mState(3) = norm_dev_ep;
mState(4) = norm_ep;
return;
}
else {
// plastic correction required
okay = false;
// determine number of active surfaces. size & fill Jact
if ( (f1 > fTOL ) && (f2 <= fTOL) ) {
// f1 surface only
Jact(0) = 1;
Jact(1) = 0;
}
else if ( (f1 <= fTOL ) && (f2 > fTOL) ) {
// f2 surface only
Jact(0) = 0;
Jact(1) = 1;
}
else if ( (f1 > fTOL ) && (f2 > fTOL) ) {
// both surfaces active
Jact(0) = 1;
Jact(1) = 1;
}
}
//-----------------MultiSurface Placity Return Map--------------------------------------
while (!okay) {
alpha1 = mAlpha1_n;
alpha2 = mAlpha2_n;
// n = eta / norm_eta; (contravaraint)
if (norm_eta < 1.0e-13) {
n.Zero();
} else {
n = eta/norm_eta;
}
// initialize R, gamma1, gamma2, dgamma1, dgamma2 = 0
R.Zero();
gamma.Zero();
dgamma.Zero();
// initialize g such that det(g) = 1
g(0,0) = 1;
g(1,1) = 1;
g(1,0) = 0;
g(0,1) = 0;
// Newton procedure to compute nonlinear gamma1 and gamma2
//initialize terms
for (int i = 0; i < 2; i++) {
if (Jact(i) == 1) {
R(0) = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1
- 9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
g(0,0) = -2*mG - two3*(mHprime + Kisoprime(alpha1)) - 9*mK*mrho*mrho_bar;
} else if (Jact(i) == 2) {
R(1) = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);
g(1,1) = -9*mK + mdelta2*T(alpha2);
}
}
if (Jact(0) == 1 && Jact(1) == 1) {
g(0,1) = -9*mK*mrho;
g(1,0) = mrho_bar*(-9*mK + mdelta2*T(alpha2));
}
g.Invert(g_contra);
// iteration counter
int m = 0;
//iterate
while ((fabs(R.Norm()) > 1e-10) && (m < 10)) {
dgamma = -1*g_contra * R;
gamma += dgamma;
//update alpha1 and alpha2
alpha1 = mAlpha1_n + root23*gamma(0);
alpha2 = mAlpha2_n + mrho_bar*gamma(0) + gamma(1);
// reset g & R matrices
g(0,0) = 1;
g(1,1) = 1;
g(1,0) = 0;
g(0,1) = 0;
R.Zero();
for (int i = 0; i < 2; i++) {
if (Jact(i) == 1) {
R(0) = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1
- 9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
g(0,0) = -2*mG - two3*(mHprime + Kisoprime(alpha1)) - 9*mK*mrho*mrho_bar;
} else if (Jact(i) == 2) {
R(1) = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);
g(1,1) = -9*mK + mdelta2*T(alpha2);
}
}
if (Jact(0) == 1 && Jact(1) == 1) {
g(0,1) = -9*mK*mrho;
g(1,0) = mrho_bar*(-9*mK + mdelta2*T(alpha2));
}
g.Invert(g_contra);
m++;
}
// check maintain Kuhn-Tucker conditions
f1 = norm_eta - (2*mG + two3*mHprime)*gamma(0) + mrho*Invariant_1
-9*mK*mrho*mrho_bar*gamma(0) - 9*mK*mrho*gamma(1) - root23*Kiso(alpha1);
f2 = Invariant_1 - 9*mK*mrho_bar*gamma(0) - 9*mK*gamma(1) - T(alpha2);
if ( count > 100 ) {
okay = true;
break;
}
// check active surfaces
if ((Jact(0) == 1) && (Jact(1) == 0)) {
// f2 may be > or < f2_tr because of softening of f2 related to alpha1
if (f2 >= fTOL) {
// okay = false;
Jact(0) = 1;
Jact(1) = 1;
count += 1;
} else {
okay = true;
}
} else if ((Jact(0) == 0) && (Jact(1) == 1)) {
// f1 will always be less than f1_tr
okay = true;
} else if ((Jact(0) == 1) && (Jact(1) == 1)) {
if ((gamma(0) <= gTOL) && (gamma(1) > gTOL)){
// okay = false;
Jact(0) = 0;
Jact(1) = 1;
count += 1;
} else if ((gamma(0) > gTOL) && (gamma(1) <= gTOL)){
// okay = false;
Jact(0) = 1;
Jact(1) = 0;
count += 1;
} else if ((gamma(0) > gTOL) && (gamma(1) > gTOL)) {
okay = true;
}
}
if ( (count > 3) && (!okay) ) {
Jact(0) = 1;
Jact(1) = 1;
count += 100;
}
if ( count > 3 ) {
opserr << "Jact = " << Jact;
opserr << "count = " << count << endln;
}
} // end of while(!okay) loop
//update everything and exit!
Vector b1(6);
Vector b2(6);
Vector n_covar(6);
Vector temp1(6);
Vector temp2(6);
// update alpha1 and alpha2
mAlpha1_n1 = alpha1;
mAlpha2_n1 = alpha2;
//update epsilon_n1_p
//first calculate n_covar
// n_a = G_ab * n^b = covariant
n_covar(0) = n(0);
n_covar(1) = n(1);
n_covar(2) = n(2);
n_covar(3) = 2*n(3);
n_covar(4) = 2*n(4);
n_covar(5) = 2*n(5);
mEpsilon_n1_p = mEpsilon_n_p + (mrho_bar*gamma(0) + gamma(1))*mI1 + gamma(0)*n_covar;
Invariant_ep = mEpsilon_n1_p(0)+mEpsilon_n1_p(1)+mEpsilon_n1_p(2);
norm_ep = sqrt(mEpsilon_n1_p(0)*mEpsilon_n1_p(0) + mEpsilon_n1_p(1)*mEpsilon_n1_p(1) + mEpsilon_n1_p(2)*mEpsilon_n1_p(2)
+ 0.5*(mEpsilon_n1_p(3)*mEpsilon_n1_p(3) + mEpsilon_n1_p(4)*mEpsilon_n1_p(4) + mEpsilon_n1_p(5)*mEpsilon_n1_p(5)));
dev_ep = mEpsilon_n1_p - one3*Invariant_ep*mI1;
norm_dev_ep = sqrt(dev_ep(0)*dev_ep(0) + dev_ep(1)*dev_ep(1) + dev_ep(2)*dev_ep(2)
+ 0.5*(dev_ep(3)*dev_ep(3) + dev_ep(4)*dev_ep(4) + dev_ep(5)*dev_ep(5)));
// update sigma
mSigma -= (3*mK*mrho_bar*gamma(0) + 3*mK*gamma(1))*mI1 + 2*mG*gamma(0)*n;
s -= 2*mG*gamma(0) * n;
Invariant_1 -= 9*mK*mrho_bar*gamma(0) + 9*mK*gamma(1);
//mSigma = s + Invariant_1/3.0 * mI1;
//update beta_n1
mBeta_n1 = mBeta_n - (two3*mHprime*gamma(0))*n;
//eta_n+1 = s_n+1 - beta_n+1;
eta = s - mBeta_n1;
norm_eta = sqrt(eta(0)*eta(0) + eta(1)*eta(1) + eta(2)*eta(2) + 2*(eta(3)*eta(3) + eta(4)*eta(4) + eta(5)*eta(5)));
// update Cep
// note: Cep is contravariant
if ((Jact(0) == 1) && (Jact(1) == 0)) {
b1 = 2*mG*n + 3*mK*mrho*mI1;
b2.Zero();
} else if ((Jact(0) == 0) && (Jact(1) == 1)){
b1.Zero();
b2 = 3*mK*mI1;
} else if ((Jact(0) == 1) && (Jact(1) == 1)){
b1 = 2*mG*n + 3*mK*mrho*mI1;
b2 = 3*mK*mI1;
}
temp1 = g_contra(0,0)*b1 + g_contra(0,1)*b2;
temp2 = mrho_bar*temp1 + g_contra(1,0)*b1 + g_contra(1,1)*b2;
NormCep = 0.0;
for (int i = 0; i < 6; i++){
for (int j = 0; j < 6; j++) {
mCep(i,j) = mCe(i,j)
+ 3*mK * mI1(i)*temp2(j)
+ 2*mG * n(i)*temp1(j)
- 4*mG*mG/norm_eta*gamma(0) * (mIIdev(i,j) - n(i)*n(j));
NormCep += mCep(i,j)*mCep(i,j);
}
}
if ( NormCep < 1e-10){
mCep = 1.0e-3 * mCe;
opserr << "NormCep = " << NormCep << endln;
}
mState(0) = Invariant_1;
mState(1) = norm_eta;
mState(2) = Invariant_ep;
mState(3) = norm_dev_ep;
mState(4) = norm_ep;
return;
}
Mat EkfSlam::getRobot()
{
return mState(Rect(0,0,1,3)).clone();
}
void EkfSlam::correct(Mat yi, Mat v, int i)
{
if(!yi.rows)
return;
double x = mState.at<double>(0); // robot x
double y = mState.at<double>(1); // robot y
double theta = mState.at<double>(2); //robot theta
Mat Li = (Mat_<double>(2,1) <<
mState.at<double>(2*i+3),
mState.at<double>(2*i+4)
);
//////////////////////////////////////////////////////////////
// setup handy data for kalman correction
Mat Hr = dh_dxb(mState(Rect(0,0,1,3)), Li);
Mat Hl = dh_dLi(mState(Rect(0,0,1,3)), Li);
Mat Hr_t = Hr.t();
Mat Hl_t = Hl.t();
Mat H;
H.push_back(Hr_t);
H.push_back(Hl_t);
H = H.t();
Mat s2 = s.clone();
s2.at<double>(0) *= s2.at<double>(0);
s2.at<double>(1) *= s2.at<double>(1);
Mat R = Mat::diag(s2);
/////////////////////////////////////////////////////////////
// Kalman Correction
Mat zbar = yi-h(mState(Rect(0,0,1,3)), Li);
if(zbar.at<double>(1) > M_PI)
zbar.at<double>(1) -= 2*M_PI;
if(zbar.at<double>(1) < -M_PI)
zbar.at<double>(1) += 2*M_PI;
///////////////////////////////////////
Mat Prl = Prm(Rect(2*i,0,2,3)).clone();
Mat Plr = Pmr(Rect(0,2*i,3,2)).clone();
Mat Pll = Pmm(Rect(2*i,2*i,2,2)).clone();
Mat Pl;
Mat pTemp1, pTemp2;
Pl = Prr.t();
pTemp1 = Prl.t();
Pl.push_back(pTemp1);
Pl = Pl.t();
pTemp1 = Plr.t();
pTemp2 = Pll.t();
pTemp1.push_back(pTemp2);
pTemp1 = pTemp1.t();
Pl.push_back(pTemp1);
///////////////////////////////////////
Mat Z = H*Pl*H.t()+R;
Mat t = zbar.t()*Z.inv()*zbar;
// update map cov
// In large maps with small uncertainties you may want to limit
// the updates to within a certain range of uncertainty with an
// if statement around this next block of code.
// if(t.at<double>(0) < 9) {
Mat P = Prr.t();
P.push_back(Plr);
P = P.t();
Mat t2 = Prm.clone();
Mat Plm = Pmm(Rect(0,2*i,Pmm.cols,2)).clone();
t2.push_back(Plm);
t2 = t2.t();
P.push_back(t2);
Mat K = P*H.t()*Z.inv();
mState += K*zbar;
Mat Pmod = K*Z*K.t();
Prr -= Pmod(Rect(0,0,3,3));
Prm -= Pmod(Rect(3,0,Pmod.cols-3,3));
//Pmr = Prm.t();
Pmr -= Pmod(Rect(0,3,3,Pmod.rows-3));
Pmm -= Pmod(Rect(3,3,Pmod.cols-3,Pmod.rows-3));
// }
}