Add kalmanfilter for angle on 20140625