KalmanFilter

Chair for Computer Aided Medical Procedures & Augmented Reality
Lehrstuhl für Informatikanwendungen in der Medizin & Augmented Reality

Sensor Fusion using the Kalman Filter

Overview

Goal of this project is to do a fusion of magnetic and optic sensor data via Extended and Federated Kalman Filters. The given data consists of positional data (x,y,z) and orientation data given as quaternions r =(r1,r2,r3,r4).

For more details on Quaternions and Kalman Filters we refer to the literature below.

Approach

We start with a simple approach using only positional data for tracking and regarding only one filter. Then step by step this sytem is extended. Rotation is included and the linear model is replaced by a general one. Next two filters are combined and finally the federated Kalman Filter is included.

Linear Kalman Filter for position tracking only

  • State Vector: The state vector x is a 9x1 vector containing the positional data (x,y,z) with its first and second derivatives (velocity and acceleration)
    statevector_position.jpg
  • Process Model: The process model relates the state at a previous time k-1 with the current state at time k. A is called state transition matrix and w represents the normal distributed process noise with covariance Q. The optinal control input Bu is discarded in the second equation.
    processmodel.jpg
    processmodel_position.jpg
  • Measurement Model: The measurement model relates the current state to the measurement z with the matrix H. v is the normal distributed measurement noise with covariance R.
    measurementmodel.jpg
    measurementmodel_position.jpg
  • Kalman Filter Cycle: The filter equations can be divided in a prediction and a correction step. The prediction step projects forward the current state and covariance to obtain an a priori estimate. After that the correction step uses the incorporates a new measurement to get an improved a posteriori estimate.
    updates_discrete2.jpg
  • Influence of the covariance matrices Q and R
    • Q, the process noise covariance, contributes to the overall uncertainty. When Q is large, the Kalman Filter tracks large changes in the data more closely than for smaller Q.
    • R, the measurement noise covariance, determines how much information from the measurement is used. If R is high, the Kalman Filter considers the measurements as not very accurate. For smaller R it will follow the measurements more closely.

The three images below visualize the positional data. The red lines represent the measurement data, the green lines are the estimated states.

Q small, R large Q and R equal Q large, R small
q-1_r1.jpg q0_r0.jpg q1_r-1.jpg

Linear Kalman Filter for position and orientation tracking

Including orientation as euler angles allows us to keep the linear model from above. The angles can be derived from the quaternion 4-vector by the following formulas.
quat2euler.jpg

  • State Vector: The state vector x now is expanded. Rotation is added in form of three euler angles (roll, pitch, jaw) together with their first and second derivatives (angular velocity and acceleration).
    statevector_pos_or.jpg
  • Process Model: The process model stays linear and connects position and rotation information of the previous and the next state.
    processmodel_pos_or1.jpg
    processmodel_pos_or2.jpg
  • Measurement Model:
    measurementmodel_pos_or.jpg

Extended Kalman Filter for position and orientation tracking

As the rotational data is given in form of quaternions the upper representation is unfavourable. But updating a state vector which contains quaternions requires a non-linear model. Heren the extended Kalman Filter can be applied which linearizes about the current mean and covariance.

  • State Vector: The rotation entries of the state vector are modified. x now contains the 9 elements for position, 4 elements of the quaternion and the 6 elements for angular velocity and acceleration as before.
    statevector_EKF.jpg
  • General Process and Measurement Models: The models for the extended Kalman filter are given by the functions f and h.
    procmeasmodel_EKF.jpg
  • Detailed Process and Measurement Models: The state vector is given as
    statevector_EKF2.jpg
    The function f handles the positional data in the linear way as described above. The quaternion r is updated through a quaternion multiplication with d which represents the difference caused by angular velocity. The angular velocity and acceleration can again be computed linearly.
    The function h has to extract the position (x,y,z) and the quaternion data r from the state vector. For the position this can be done directly, the rotation quaternion needs an additional normalization. procmeasmodel_EKF2.jpg
  • Kalman Filter Cycle: To take account of the non-linear models the equations for the filter cycle are slightly modified.
    updates_EKF.jpg
    The matrices A, H, W, V are Jacobians with partial derivatives of the functions f and h.
    jacobians_EKF.jpg

Fusion of two 6DOF trackers using the Kalman Filter

Fusion of two 6DOF trackers using the federated Kalman Filter

Literature

-- KatharinaPentenrieder - 08 Mar 2005



Edit | Attach | Refresh | Diffs | More | Revision r1.13 - 06 Jun 2005 - 12:55 - MartinBauer

Lehrstuhl für Computer Aided Medical Procedures & Augmented Reality    rss.gif