Question in the PositionVelocityEstimator.cpp file

[ZenanZhu]

Hi,

I'm trying to understand this state estimation file, however, I have some difficulties in line 123 ~ 149. What I learned so far, vs is the velocity of the foot reltive to the body and pzs is the contact height of each foot, however, there is a term trust in the equations at line 148 ~ 149:

    _vs.segment(i1, 3) = (1.0f - trust) * v0 + trust * (-dp_f);
    pzs(i) = (1.0f - trust) * (p0(2) + p_f(2));

I am not sure about the definatoin of trust in line 123 ~ 132. How does it relate with vs and pzs?

    T trust = T(1);
    T phase = fmin(this->_stateEstimatorData.result->contactEstimate(i), T(1));
    //T trust_window = T(0.25);
    T trust_window = T(0.2);

    if (phase < trust_window) {
      trust = phase / trust_window;
    } else if (phase > (T(1) - trust_window)) {
      trust = (T(1) - phase) / trust_window;
    }

And also, what is the character of high_suspect_number in the equations of Q and R matrices?

    Q.block(qindex, qindex, 3, 3) =
        (T(1) + (T(1) - trust) * high_suspect_number) * Q.block(qindex, qindex, 3, 3);
    R.block(rindex1, rindex1, 3, 3) = 1 * R.block(rindex1, rindex1, 3, 3);
    R.block(rindex2, rindex2, 3, 3) =
        (T(1) + (T(1) - trust) * high_suspect_number) * R.block(rindex2, rindex2, 3, 3);
    R(rindex3, rindex3) =
        (T(1) + (T(1) - trust) * high_suspect_number) * R(rindex3, rindex3);

[xdaumich]

Hi Zenan,

To start with the state estimator, please check this paper for the derivation. Page 7 explains the Cheetah's pose estimator. https://www.researchgate.net/publication/329759867_MIT_Cheetah_3_Design_and_Control_of_a_Robust_Dynamic_Quadruped_Robot

The trust and phase are relative to the stance/swing switching. Trust is low (~0) when the leg is swing leg and it is high (~1) when the leg is stance leg. vs and pzs are defined in the paper. Similarly, the high_suspect_number is for the swing leg. The high_suspect_number assign high noise for Q and R, meaning "do not use them in the estimation".

[qjones81]

Hi I have a follow up question. I see something here I do not believe I saw in the paper.

Eigen::Matrix<T, 18, 18> Pt = _P.transpose();
  _P = (_P + Pt) / T(2);

  if (_P.block(0, 0, 2, 2).determinant() > T(0.000001)) {
    _P.block(0, 2, 2, 16).setZero();
    _P.block(2, 0, 16, 2).setZero();
    _P.block(0, 0, 2, 2) /= T(10);
  }

What is the reasoning/issue behind needing this heuristic on the error covariances? I have run the code both ways and do not see noticeable difference in the simulator. Still a little green on Kalman Filters I am curious if these are just some details you need to make them more stable and reliable.

Thanks,

[xdaumich]

@qjones81 , simply put, the first two elements about the x, y position. There is no direct measurement, so they are integrated from the velocity. Theoretically, the P.block(0, 0, 2, 2) should be zero, but there would numerically error make them non-zero. So you need to manually make them zero. Otherwise, the x, y position will diverge.

[benjirer]

Sorry do grab up this old issue again, but I still don't quite understand why we are manually adapting P in this last step. Especially I don't understand why we are dividing the x and y position error covariance by ten and not setting it to zero?

[Harvey2019]

@benjirer _P should be a symmetric matrix, but it may be asymmetry after computed. By this step, we try to correct it， I think.

[chinahuangyong]

Hello everyone, why does the matrix Q have a coefficient of 20.0f when the position and velocity estimator by using kalman filter.

_Q0.block(0, 0, 3, 3) = (dt / 20.f) Eigen::Matrix<T, 3, 3>::Identity(); _Q0.block(3, 3, 3, 3) = (dt 9.8f / 20.f) * Eigen::Matrix<T, 3, 3>::Identity();