The Kalman Gain

The last and final equation is the Kalman Gain Equation.

The Kalman Gain in matrix notation is given by:

\[ \boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} } \]
where:
\( \boldsymbol{K_{n} } \) is the Kalman Gain
\( \boldsymbol{P_{n,n-1}} \) is a prior estimate uncertainty (covariance) matrix of the current sate (predicted at the previous state)
\( \boldsymbol{H} \) is an observation matrix
\( \boldsymbol{R_{n}} \) is a Measurement Uncertainty (measurement noise covariance matrix)

Kalman Gain Equation Derivation

I will provide the derivation of the Kalman Gain Equation. If you don’t bother about the derivation, you can jump to the next topic.

First, let’s open the Covariance Update Equation:

Notes
\( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} \color{blue}{\boldsymbol{ \left( I - K_{n}H \right)^{T}}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) Covariance Update Equation
\( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} \color{blue}{\boldsymbol{ \left( I - \left( K_{n}H \right)^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) \( \boldsymbol{I^{T} = I} \)
\( \boldsymbol{P_{n,n}} = \color{green}{\boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}}} \color{blue}{\boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) Apply the matrix transpose property: \( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \)
\( \boldsymbol{P_{n,n}} = \color{green}{\boldsymbol{ \left( P_{n,n-1} - K_{n}H P_{n,n-1} \right)}} \boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \)
\( \boldsymbol{P_{n,n}} = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + \\ + \color{#7030A0}{\boldsymbol{K_{n}H P_{n,n-1}H^{T}K_{n}^{T} + K_{n} R_{n} K_{n}^{T} } } \) Open brackets
\( \boldsymbol{P_{n,n}} = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + \\ + \color{#7030A0}{\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } } \) Group the last two terms

Kalman Filter is an optimal filter. Thus, we will seek for Kalman Gain that minimizes the estimate variance.

In order to minimize the estimate variance, we need to minimize the main diagonal (from the upper left to the lower right) of the covariance matrix \( \boldsymbol{P_{n,n}} \).

The sum of the main diagonal of the square matrix is the trace of the matrix. Thus, we need to minimize \( tr(\boldsymbol{P_{n,n}}) \). In order to find conditions for the minimum, we will differentiate the trace of \( \boldsymbol{P_{n,n}} \) with respect to \( \boldsymbol{K_{n}} \) and set the result to zero.

Notes
\( tr\left( \boldsymbol{P_{n,n}} \right) = tr\left( \boldsymbol{P_{n,n-1}}\right) - \\ - \color{#FF8C00}{tr\left( \boldsymbol{ P_{n,n-1}H^{T}K_{n}^{T} }\right) - tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + \\ + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \) Trace of Covariance Update Equation
\( tr\left( \boldsymbol{P_{n,n}} \right) = tr\left( \boldsymbol{P_{n,n-1}}\right) - \color{#FF8C00}{ 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + \\ + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \) The trace of the matrix is equal to the trace of its transpose
\( \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} = \color{blue}{\frac{d\left( tr\left( \boldsymbol{P_{n,n-1}}\right) \right)}{d\boldsymbol{K_{n}}}} - \color{green}{ \frac{d\left( 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right) \right) }{d\boldsymbol{K_{n}}} } + \\ + \color{#7030A0}{\frac{ d\left( tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \right) }{d\boldsymbol{K_{n}}}} = 0 \) Differentiate the trace of \( \boldsymbol{P_{n,n}} \) with respect to \( \boldsymbol{K_{n}} \)
\( \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} = \color{blue}{0} - \color{green}{ 2 \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } + \\ + \color{#7030A0}{\boldsymbol{2K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } = 0 \) Matrix Calculus rules:
\( \color{green}{\frac{d}{\boldsymbol{A}} \left( tr\left( \boldsymbol{AB} \right) \right) = \boldsymbol{B}^{T} } \\ \color{#7030A0}{\frac{d}{\boldsymbol{A}} \left( tr\left( \boldsymbol{ABA^{T}} \right) \right) = 2\boldsymbol{AB} } \)
\( \color{green}{ \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } = \color{#7030A0}{\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } \)
\( \boldsymbol{K_{n}} = \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \)
\( \boldsymbol{K_{n}} = \boldsymbol{ P_{n,n-1}^{T} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \) Apply the matrix transpose property: \( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \)
\( \boldsymbol{K_{n}} = \boldsymbol{ P_{n,n-1} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \) Covariance matrix is a symmetric matrix: \( \boldsymbol{ P_{n,n-1}^{T}} = \boldsymbol{ P_{n,n-1}} \)
Previous Next