The Kalman Gain

The 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 the prior estimate uncertainty (covariance) matrix of the current state (predicted at the previous step)
\( \boldsymbol{H} \) is the observation matrix
\( \boldsymbol{R_{n}} \) is the Measurement Uncertainty (measurement noise covariance matrix)

Kalman Gain Equation Derivation

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

First, let’s rearrange 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} } } \) Expand
\( \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

The Kalman Filter is an optimal filter. Thus, we will seek a 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 the conditions required to produce a 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 the 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 \) \( \color{green}{\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{AB} \right) \right) = \boldsymbol{B}^{T} } \\ \color{#7030A0}{\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{ABA^{T}} \right) \right) = 2\boldsymbol{AB} } \)
See the proof here.
\( \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} } \) The Covariance matrix is a symmetric matrix: \( \boldsymbol{ P_{n,n-1}^{T}} = \boldsymbol{ P_{n,n-1}} \)
Previous Next