Put measurement update into separate function
Add acceleration update to last measurement update