GPSAccKalmanFilter fails if useGpsSpeed=false

[dsame]

in the constructor there's a code

int mesDim = useGpsSpeed ? 4 : 2;
...
m_kf = new KalmanFilter(4, mesDim, 2);

so KalmanFilter creates Zk with 2 rows

        this.Zk = new Matrix(measureDimension, 1);

but there's a call of Zk.setData with 4 varargs

    public void update(double timeStamp,
                       double x,
                       double y,
                       double xVel,
                       double yVel,
                       double posDev,
                       double velErr) {
   ... 
       m_kf.Zk.setData(x, y, xVel, yVel);
   ... 

which makes assert to fail

    public void setData(double... args) {
/* 4 !=  2 x 1 */
        assert(args.length == rows * cols);
...
    }

[Lezh1k]

There is fix in lib branch in C code. But I don't have time to finish this.