Kalman filter using CMSIS DSP