Dismiss Notice
Join Physics Forums Today!
The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

Kalman Filter Help

  1. May 8, 2006 #1
    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.
  2. jcsd
  3. May 9, 2006 #2


    User Avatar

    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.
  4. May 10, 2006 #3
    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?
  5. May 10, 2006 #4


    User Avatar

    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.
  6. May 11, 2006 #5

    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.
  7. May 12, 2006 #6


    User Avatar

    You can model your 2D movement as:
    [tex]x_{k+1} = x_k + vx_k T + \frac{1}{2}ax_k T^2 + \nu_x[/tex]
    [tex]y_{k+1} = y_k + vy_k T + \frac{1}{2}ay_k T^2 + \nu_y[/tex]
    [tex]vx_{k+1} = vx_k + ax_k T + \nu_vx[/tex]
    [tex]vy_{k+1} = vy_k + ay_k T + \nu_vy[/tex]
    [tex]ax_{k+1} = ax_k + \nu_ax[/tex]
    [tex]ay_{k+1} = ay_k + \nu_ay[/tex]
    Or,in matrix form:
    [tex]\mathbf{x_{k+1}} = \mathbf{\Phi} \mathbf{x_k} + \mathbf{w_k}[/tex]
    [tex]\mathbf{x_k} = \left[ \begin{array}{c}
    \end{array} \right][/tex]
    [tex]\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][/tex]
    [tex]\mathbf{w_k}= \left[ \begin{array}{c}
    \end{array} \right][/tex]
    [tex]\mathbf{w_k}[/tex] represents the process noise (uncertainties in the movement).
    The measurement equation, if I understand correctly is:
    [tex]\mathbf{z_k} = \left[ \begin{array}{c}
    \end{array} \right][/tex]
    [tex]\mathbf{z_k} = \mathbf{H} \mathbf{x_k}[/tex]
    [tex]\mathbf{H} = \left[ \begin{array}{ccccc}
    1 & 0 & 0 & 0 & 0 & 0\\
    0 & 0 & 0 & 1 & 0 & 0
    \end{array} \right][/tex]
    If you know an estimate [tex]\mathbf{\hat{x}_{k|k}}[/tex] of [tex]\mathbf{x_k}[/tex] at the instant [tex]t_k[/tex] and have a measurement [tex]\mathbf{z_{k+1}} [/tex] at instant [tex]t_{k+1}[/tex] , the best estimate [tex]\mathbf{\hat{x}_{k+1|k+1}}[/tex] of [tex]\mathbf{x_{k+1}}[/tex] is given by the Kalman filter:
    [tex]\mathbf{\hat{x}_{k+1|k+1}} = [tex]\mathbf{\hat{x}_{k+1|k}} + \mathbf{K_{k+1}}(\mathbf{z_{k+1}} - \mathbf{H} \mathbf{\hat{x}_{k+1|k}})[/tex]
    [tex]\mathbf{\hat{x}_{k+1|k}} = \mathbf{\Phi} \mathbf{\hat{x}_{k|k}}[/tex]
    [tex]\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}[/tex]
    [tex]\mathbf{P_{k+1|k+1}} = (I - \mathbf{K_{k+1}}.\mathbf{H}). \mathbf{P_{k+1|k}}[/tex]
    [tex]\mathbf{P_{k+1|k}} = \mathbf{\Phi}.\mathbf{P_{k|k}}.\mathbf{\Phi^T} + \mathbf{Q}[/tex]
    \mathbf{\hat{x}_{k|j}}[/tex] is the best estimate of [tex]\mathbf{x_k}[/tex] given the measurements until the instant [tex]t_j[/tex].
    [tex]\mathbf{P_{k|j}} = E[(\mathbf{x_k}-\mathbf{\hat{x}_{k|j}}).(\mathbf{x_k}-\mathbf{\hat{x}_{k|j}})^T][/tex]
    is the estimation error covariance matrix.
    [tex]\mathbf{Q}[/tex] is the process noise covariance matrix
    [tex]\mathbf{R}[/tex] is the measurement noise covariance matrix.
    Last edited by a moderator: May 12, 2006
Know someone interested in this topic? Share this thread via Reddit, Google+, Twitter, or Facebook

Have something to add?

Similar Discussions: Kalman Filter Help
  1. Kalman filter (Replies: 8)