Kalman Filter Help: Get Started Estimating Future Location of Mobile Device

[lynchtkd]

Hi all,

I am trying to estimate the future location of a mobile device and I was thinking of using the Kalman Filter to do this. Problem is I don't really know where to start. All the literature I have been reading gets much to advanced to quickly.

What i have is basically, I am measuring the angle between lines (measuring movements at time intervals) so we take a set of recent movements and extrapolate the likely future location of the mobile device.

Can anyone help please.

 

[SGT]

Hi all,

I am trying to estimate the future location of a mobile device and I was thinking of using the Kalman Filter to do this. Problem is I don't really know where to start. All the literature I have been reading gets much to advanced to quickly.

What i have is basically, I am measuring the angle between lines (measuring movements at time intervals) so we take a set of recent movements and extrapolate the likely future location of the mobile device.

Can anyone help please.

Is the sensor located at the mobile and measures the angles to other objects, or is the sensor located elsewhere and measures angles to the mobile?
In the second case, the system is observable only if the sensor follows an accelerated trajectory.
In order to help you I must have more informations about your system.

 

[lynchtkd]

As the device moves we track its location, with say GPS. At regular intervals we read the location. So for example we have three points A,B,C. We create the lines |AB| and |BC| and measure the angle of change in direction from one line segment to another.

Am I right in thinking that using/implementing a Kalman filter we can use it to estimate the future location of the user?

 

[SGT]

As the device moves we track its location, with say GPS. At regular intervals we read the location. So for example we have three points A,B,C. We create the lines |AB| and |BC| and measure the angle of change in direction from one line segment to another.

Am I right in thinking that using/implementing a Kalman filter we can use it to estimate the future location of the user?

Do you intend to estimate the motion parameters with only 3 measurements? It is not enough. But if you have several measurements at known times, you can use a Kalman filter in order to estimate the correct position (the measurements allways contain errors) as well as velocity and acceleration. Knowing those dynamic parameters you can estimate the future position.
The segments |AB|, |BC| and the angle between them are irrelevant to the use of a Kalman filter.

 

[lynchtkd]

Cheers,

I only gave three points as an example, but using Kalman would be feasible to estimate future location. How would I go about implementing that model given I can know the velocity and acceleration? I have been reading and reading to try and get a greater understanding of the Kalman filter, but if I could get a more grounded example it would help greatly.

 

[SGT]

You can model your 2D movement as:
$$x_{k+1} = x_k + vx_k T + \frac{1}{2}ax_k T^2 + \nu_x$$
$$y_{k+1} = y_k + vy_k T + \frac{1}{2}ay_k T^2 + \nu_y$$
$$vx_{k+1} = vx_k + ax_k T + \nu_vx$$
$$vy_{k+1} = vy_k + ay_k T + \nu_vy$$
$$ax_{k+1} = ax_k + \nu_ax$$
$$ay_{k+1} = ay_k + \nu_ay$$
Or,in matrix form:
$$\mathbf{x_{k+1}} = \mathbf{\Phi} \mathbf{x_k} + \mathbf{w_k}$$
Where
$$\mathbf{x_k} = \left[ \begin{array}{c} x_k\\vx_k\\ax_k\\y_k\\vy_k\\ay_k \end{array} \right]$$
$$\mathbf{\Phi} = \left[ \begin{array}{cccccc} 1 & T & \frac{1}{2}T^2 & 0 & 0 & 0\\ 0 & 1 & T & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & T & \frac{1}{2}T^2\\ 0 & 0 & 0 & 0 & 1 & T\\ 0 & 0 & 0 & 0 & 0 & 1 \end{array} \right]$$
$$\mathbf{w_k}= \left[ \begin{array}{c} \nu_x\\ \nu_vx\\ \nu_ax\\ \nu_y\\ \nu_vy\\ \nu_ay \end{array} \right]$$
$$\mathbf{w_k}$$ represents the process noise (uncertainties in the movement).
The measurement equation, if I understand correctly is:
$$\mathbf{z_k} = \left[ \begin{array}{c} x_k\\y_k \end{array} \right]$$
or
$$\mathbf{z_k} = \mathbf{H} \mathbf{x_k}$$
with
$$\mathbf{H} = \left[ \begin{array}{ccccc} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0 \end{array} \right]$$
If you know an estimate $$\mathbf{\hat{x}_{k|k}}$$ of $$\mathbf{x_k}$$ at the instant $$t_k$$ and have a measurement $$\mathbf{z_{k+1}}$$ at instant $$t_{k+1}$$ , the best estimate $$\mathbf{\hat{x}_{k+1|k+1}}$$ of $$\mathbf{x_{k+1}}$$ is given by the Kalman filter:
[tex]\mathbf{\hat{x}_{k+1|k+1}} = $$\mathbf{\hat{x}_{k+1|k}} + \mathbf{K_{k+1}}(\mathbf{z_{k+1}} - \mathbf{H} \mathbf{\hat{x}_{k+1|k}})$$
with
$$\mathbf{\hat{x}_{k+1|k}} = \mathbf{\Phi} \mathbf{\hat{x}_{k|k}}$$
$$\mathbf{K_{k+1}} = \mathbf{P_{k+1|k}}.\mathbf{H^T} (\mathbf{H} .\mathbf{P_{k+1|k}}.\mathbf{H^T} + \mathbf{R})^{-1}$$
$$\mathbf{P_{k+1|k+1}} = (I - \mathbf{K_{k+1}}.\mathbf{H}). \mathbf{P_{k+1|k}}$$
$$\mathbf{P_{k+1|k}} = \mathbf{\Phi}.\mathbf{P_{k|k}}.\mathbf{\Phi^T} + \mathbf{Q}$$
where
\mathbf{\hat{x}_{k|j}}[/tex] is the best estimate of $$\mathbf{x_k}$$ given the measurements until the instant $$t_j$$.
$$\mathbf{P_{k|j}} = E[(\mathbf{x_k}-\mathbf{\hat{x}_{k|j}}).(\mathbf{x_k}-\mathbf{\hat{x}_{k|j}})^T]$$
is the estimation error covariance matrix.
$$\mathbf{Q}$$ is the process noise covariance matrix
and
$$\mathbf{R}$$ is the measurement noise covariance matrix.