Porting kalman to AVR - add arithmetic functions