I want to get the distance between points. I have a measurement of latitude, longitude and height from GPS/barometer systen (which I converted into north east down coordinates) and I have another measurement of the vehicle's velocity in the north east and down direction. To find the distance, I want to set up the state space equation for a Kalman filter. I attached a picture of my attempt. In the picture, Ts is the sample period, v_k is the magnitude of the velocity, p_k is the position and D is the distance. Please correct my program.