Innovation Term Convention

[tvandenzegel]

Hi,

I noticed an inconsistency in how the innovation term y is defined between ImgUpdate.hpp and PoseUpdate.hpp, and I hope somebody could clarify it to me.

In ImgUpdate.hpp, the innovation term is computed as:

Eigen::Vector2d pixError;
pixError(0) = static_cast<double>(state.aux().feaCoorMeas_[ID].get_c().x - featureOutput_.c().get_c().x);
pixError(1) = static_cast<double>(state.aux().feaCoorMeas_[ID].get_c().y - featureOutput_.c().get_c().y);
y.template get<mtInnovation::_pix>() = pixError + noise.template get<mtNoise::_pix>();

This implies:

y = measurement - h(x) // where h(x) is the predicted pixel location

However, in PoseUpdate.hpp, the innovation term is formulated as:

y.pos() = get_IrIW(state) +
          get_qWI(state).inverseRotate(V3D(state.WrWM() + state.qWM().rotate(get_MrMV(state)))) -
          meas_.pos() + noise.pos();

Which corresponds to:

y = h(x) - measurement

Shouldn’t these follow a consistent sign convention? :)