1
$\begingroup$

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

$\endgroup$
1
$\begingroup$

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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.