Added code to do polynomial IMU measurements for kinematic model