Kalman Filter Help

[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 dont 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 dont 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:
\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}} 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.