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?