Real-Time Robotics Framework

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== Kalman Filter ======
A kalman filter block is used to estimate the state of a system. 
The system must be described in a state space representation:
<code> 
u -----------------------> physical system ------------------------------> y
    |                                                              |
    |                                                              |
    |   ---------------------> D ---------------------             |
    |  |                                              |            |
    |  |                                              | +          | -
    |  |         +      x(k-1)          x(k)       +  v   y_hat +  v
     -----> Bd ---> 0 ----------> z⁻1 -------> C ---> 0 ---------> 0
                    ^                                 |            |
                  + |                                 | +          |
                    |                                 v            |
                     -------------- Ad <------------- 0 <--- K ----
                                                         +
</code>
with 
  x - state vector
  u - input vector of the physical system
  y - output vector of the physical system
      
  Ad - time discrete system matrix
  Bd - time discrete input matrix
  C  - output matrix
  D  - feed forward matrix
  K  - kalman feedback matrix
 
Furthermore, disturbances in the system need to be modeled as noise in a separate 
vector z with a related covariance matrix G. Measurement noise is added to the
output by introducing a vector v.
 
The complete state space model is
  x(k-1) = Ad*x(k) + Bd*u(k) + Gd*z(k)
  y(k) = C*x(k) + D*u(k) + v(k)
 
For a detailed introduction to the topic, please refer to the book 
"Kalman-Filter Einführung in die Zustandsschätzung und ihre Anwendungen für 
eingebettete Systeme" by Reiner Marchthaler and Sebastian Dingler.
 
The kalman filter block has two separate blocks included. One is for the 
prediction and the other one for the correction. The correction block
should be run after reading the sensor values, while the prediction block should 
run after the input vector is defined. The two blocks can run in different time domains.