/ electronics / dsp /
Kalman Filter 
The Kalman Filter is a linear quadratic estimator used to estimate the state of a system by combining sensor measurements and a (physical) process model of the system. It works optimal if measurements and process variations have white gaussian noise.
The Kalman Filter averages a prediction of a system’s state with a new measurement using a weighted average. The purpose of the weights is that estimated values with smaller uncertainty are “trusted” more. The result of the weighted average is a new state estimate that lies between the predicted and measured state, and has a smaller estimated uncertainty than either alone.
The Kalman filter works in two steps: predict and update.
Defining the Model
- system’s process model
- control inputs to that process
- multiple sequential measurements (e.g. from sensors)
The dynamic model for the physical process is
with the states , transition matrix , gaussian process noise , input , measurements , measurement model , gaussian measurement noise , time point .
The measurement matrix defines how the observations correspond to the state. If the state variables can be directly observed then .
If there are no known inputs to the process, then and this term can be removed.
The overall uncertainty of the estimation is expressed with the covariance matrix . This matrix is influenced by the process noise and the measurement noise . During prediction, the process noise increases the uncertainty, whereas combining it with the measurement during the update phase decreases the uncertainty.
If values of are larger than values of , the filter trusts more the process, less the measurements.
1. Step: Prediction
1.1 calculate the new (a priori) state estimate based on the old state and the dynamic model (e.g. physical laws)
1.2 calculate a new process covariance (how certain is the model?)
2. Step: Update
2.1 calculate intermediate values (optional):
Innovation: which are the real measurements minus predicted measurements
2.2 calculate optimal Kalman-gain:
2.3 calculate the updated (a posteriori) state estimate using measurements:
which is the estimation of based on with where 0.0 means the filter fully trusts the prediction and 1.0 means the filter fully trusts the measurement.
2.4 update process covariance:
Extended Kalman Filter
The Extended Kalman Filter (EKF) uses non-linear dynamic models.
where and are non-linear functions. For covariance the Jacobi-Matrix of the model is used:
If sensors measure the same state , this can be expressed in the measurement matrix . The column will have rows with entries.
Kalman Filter for Gyroscope and Accelerometer:state are the orientation angles roll and pitch and the bias angle