UKF control of state vector missing?

[wienans]

Hey Guys,

i was wondering if the ukf doesn't support control of the state vector or if it isn't implemented completely. In ukf.ccp the function prepareControl(referenceTime, delta); is called which writes to the variable controlAcceleration_ but controlAcceleration_ isn't used later in the code, or i can't find the position where the control is added. in the ekf.cpp there is a block directly before the state transfer

// (1) Apply control terms, which are actually accelerations
    state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
    state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
    state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;
 state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
      controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));
    state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
      controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));
    state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
      controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));

Shouldn't the ukf.cpp include something similar to this too? Thanks for help

[ayrton04]

Yeah, it should do something like that, though not exactly the same, because prediction is done very differently in the UKF. I'll mark this as a bug. Thanks!

[lkdo]

Hi,

What does it mean that prediction is done very differently in the UKF ?

What am I missing ? To account for the control term, isn't it as simple as below ?

For each sigma point prediction (taking into account the fixes of https://github.com/cra-ros-pkg/robot_localization/issues/621 ),

void Ukf::projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta) { ...

sigmaPoint(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
sigmaPoint(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
sigmaPoint(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;

sigmaPoint(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
  controlAcceleration_(ControlMemberVx) : sigmaPoint(StateMemberAx));
sigmaPoint(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
  controlAcceleration_(ControlMemberVy) : sigmaPoint(StateMemberAy));
sigmaPoint(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
  controlAcceleration_(ControlMemberVz) : sigmaPoint(StateMemberAz));

sigmaPoint.applyOnTheLeft(transferFunction_);

}

thx,

[ayrton04]

What does it mean that prediction is done very differently in the UKF ?

UKFs, as you are apparently aware, use sigma points rather than project a single state. That, to me, is very different than an EKF. So as I said, the logic would be like the EKF, but "not exactly the same."