first draft of new kalamn where x= [p12 v1 v2]T