doing kalman with only 6x6