# Kalman filter prediction ahead of next measurement

I want to implement a Kalman Filter for predicting x/y positions. I have a sensor which gives me the current position (noisy). Now I want to smooth and predict the position. Thus Kalman Filter came to my mind.

How do I have to design the filter taking the time in regards? I want to predict the next state which is ahead of the upcoming measurement. Moreover since i do not have a velocity, I'd like to estimate the velocity by the kalman as well.

State := [xpos,ypos,xvelocity,yvelocity]
Measurement := [xpos,ypos]
ControlInput := predictionTime


I would run the following algorithm, whenever i get a measurement:

1. Measure
2. Update kalman gain
3. Predict
4. Get State estimate


Now, when i use a predictionTime which is newer than the next measurement, the next measurement does not fit to the predicted state.

Is there a strategy to solve this issue? My predicted state and my next measurement do not fit, how could I fix this?

Thanks

Basically what you would do is the following:

Let:

$$X = [x,y,vx,vy]$$

$$P = \begin{matrix} \sigma^2_x & 0 & 0 & 0 \\ 0 & \sigma^2_y & 0 & 0 \\ 0 & 0 & \sigma^2_{vx} & 0 \\ 0 & 0 & 0 & \sigma^2_{vy} \\ \end{matrix}$$ $$\Delta t$$ Be your state vector, state covariance matrix, and time interval between two measurements respectively.

How can you transition from one state K to another state K+1,, which is Dt seconds later? Taking the simple equation. $$distance (meters) = speed (meters / sec)* time (secs)$$

You see that transitioning from one state to another is done in the following way:

$$F_k = \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix}$$

Because, when you do the state prediction, you apply the following equation (simplyfication with no input control vector):

$$X_k = F_k * X_{k-1}$$

Multiplying both matrixes together yields back the following equations:

$$x_k = x_{k-1} + vx_{k-1}* \Delta t$$

$$y_k = y_{k-1} + vy_{k-1}* \Delta t$$

$$vx_k = vx_{k-1}$$

$$vy_k = vy_{k-1}$$

If you need further clarification on another aspect, please do not hesitate to ask.