When 'useGpsSpeed' is 'false', m_kf.Zk is size 2 but update expects size 4

[Pstoppani]

Apparently 'assert' is turned off so the app does not crash even though 'm_kf.Zk.setData(x, y, xVel, yVel);' is passing 4 values when the Zk is only a 2 element vector.

public void update(double timeStamp, double x, double y,
                       double xVel,  double yVel,
                       double posDev,  double velErr) {
        m_predictCount = 0;
        m_timeStampMsUpdate = timeStamp;
        rebuildR(posDev, velErr);
        m_kf.Zk.setData(x, y, xVel, yVel);  // <--- Zk has 2 elements, not 4
        m_kf.update();
   }

[Pstoppani]

I believe the fix is to add a check for m_useGpsSpeed:

    if (m_useGpsSpeed) {
        m_kf.Zk.setData(x, y, xVel, yVel);
    } else {
        m_kf.Zk.setData(x, y);
    }

[Lezh1k]

Sure, thank you