Least Squares Method – Intro to Kalman Filter

Consider a Linear Equation,

$y_{i} = \sum_{j = 1}^{n} C_{i, j} x_{j} + v_{i}, i = 1, 2, \dots$

, where $C_{i,j}$ are scalars and $v_i\in \mathbb{R}$ is the measurement noise. The noise is unknown, while we assume it follows certain patterns (the assumptions are due to some statistical properties of the noise). We assume $v_i, v_j$ are independent for $i\neq j$ . Properties are mean of zero, and variance equals sigma squared.

$E (v_{i}) = 0$

$E (v_{i}^{2}) = σ_{i}^{2}$

We can rewrite $y_i = \sum_{j=1}^n C_{i,j} x_j +v_i$ as,

$(\begin{matrix} y_{1} y_{2} ⋮ y_{s} \end{matrix}) = (\begin{matrix} C_{11} & C_{12} & \dots & C_{1 n} C_{21} & C_{22} & \dots & C_{2 n} ⋮ & ⋮ & \dots & ⋮ C_{s 1} & C_{s 2} & \dots & C_{s n} \end{matrix}) (\begin{matrix} x_{1} x_{2} ⋮ x_{n} \end{matrix}) + (\begin{matrix} v_{1} v_{2} ⋮ v_{s} \end{matrix})$

, in a matrix form,

$\vec{y} = C \vec{x} + \vec{v}$

, but I would write in a short form,

$y = C x + v$

We solve for the least squared estimator from the optimisation problem, (there is a squared L2 norm)

$min_{x} | | y - C x | |_{2}^{2}$

Recursive Least Squared Method

The classic least squared estimator might not work well when data evolving. So, there emerges a Recursive Least Squared Method to deal with the discrete-time instance. Let’s say, for a discrete-time instance $k$ , $y_k \in \mathbb{R}’$ is within a set of measurements group follows,

$y_{k} = C_{k} x + v_{k}$

, where $C_k \in \mathbb{R}^{l\times n}$ , and $v_k \in \mathbb{R}^l$ is the measurement noise vector. We assume that the covariance of the measurement noise is given by,

$E [v_{k} v_{k}^{T}] = R_{k}$

, and

$E [v_{k}] = 0$

The recursive least squared method has the following form in this section,

$\hat{x} k = \hat{x} k - 1 + K_{k} (y_{k} - C_{k} {\hat{x}}_{k - 1})$

, where $\hat{x}k$ and $\hat{x}{k-1}$ are the estimates of the vector $x$ at the discrete-time instants $k$ and $k-1$ , and $K_k \in \mathbb{R}^{n\times l}$ is the gain matrix that we need to determine. $K_k$ is coined the ‘Gain Matrix’

The above equation updates the estimate of $x$ at the time instant $k$ on the basis of the estimate $\hat{x}_{k-1}$ at the previous time instant $k-1$ and on the basis of the measurement $y_k$ obtained at the time instant $k$ , as well as on the basis of the gain matrix $K_k$ computed at the time instant $k$ .

Notation

$\hat{x}$ is the estimate.

$\hat{x} k = (\begin{matrix} \hat{x} 1, k \hat{x} 2, k ⋮ \\ h a t x n, k \end{matrix})$

, which is corresponding with the true vector $x$ .

$x = (\begin{matrix} x_{1} x_{2} ⋮ x_{n} \end{matrix})$

The estimation error, $\epsilon_{i,k} = x_i – \hat{x}_{i,k} \quad i=1,2,…,n$ .

$ϵ_{k} = (\begin{matrix} ϵ_{1, k} ϵ_{2, k} ⋮ \\ e p s i l o n_{n, k} \end{matrix}) = x - {\hat{x}}_{k} = (\begin{matrix} x_{1} - {\hat{x}}_{1, k} x_{2} - {\hat{x}}_{2, k} ⋮ \x_{n} - {\hat{x}}_{n, k} \end{matrix})$

The gain $K_k$ is computed by minimising the sum of variances of the estimation errors,

$W_{k} = E (ϵ_{1, k}^{2}) + E (ϵ_{2, k}^{2}) + \dots + E (ϵ_{n, k}^{2})$

Next, let’s show the cost function could be represented as follows, ( $tr(.)$ is the trace of a matrix)

$W_{k} = t r (P_{k})$

, and $P_k$ is the estimation error covariance matrix defined by

$P_{k} = E (ϵ_{k} ϵ_{k}^{T})$

Or, says,

$K_{k} = a r g min_{K_{k}} W_{k} = t r (E (ϵ_{k} ϵ_{k}^{T}))$

Why is that?

$ϵ_{k} ϵ_{k}^{T} = (\begin{matrix} ϵ_{1, k} ϵ_{2, k} \\ v d o t s ϵ_{n, k} \end{matrix}) (\begin{matrix} ϵ_{1, k} & ϵ_{2, k} & \dots & ϵ_{n, k} \end{matrix})$

$= (\begin{matrix} ϵ_{1, k}^{2} & \dots & ϵ_{1, k} ϵ_{n, k} ⋮ & ϵ_{i, k}^{2} & ⋮ ϵ_{1, k} ϵ_{n, k} & \dots & ϵ_{n, k}^{2} \end{matrix})$

So,

$P_{k} = E [ϵ_{k} ϵ_{k}^{T}]$

$t r (P_{k}) = E (ϵ_{1, k}^{2}) + E (ϵ_{2, k}^{2}) + \dots + E (ϵ_{n, k}^{2})$

Optimisation

$K_{k} = a r g min_{K_{k}} W_{k} = t r (E (ϵ_{k} ϵ_{k}^{T})) = t r (P_{k})$

Let’s derive the optimisation problem.

$ϵ_{k} = x - {\hat{x}}_{k}$

$= x - \hat{x} k - 1 - K_{k} (y_{k} - C_{k} \hat{x} k - 1)$

$= x - \hat{x} k - 1 - K_{k} (C_{k} x + v_{k} - C_{k} \hat{x} k - 1)$

$= (I - K_{k} C_{k}) (x - {\hat{x}}_{k - 1}) - K_{k} v_{k}$

$= (I - K_{k} C_{k}) ϵ_{k - 1} - K_{k} v_{k}$

Recall $y_k = C_k x + v_k$ and $\hat{x}k = \hat{x}{k-1} + K_k (y_k – C_k \hat{x}_{k-1})$

So, $\epsilon_k \epsilon_k^T$ would be,

$ϵ_{k} ϵ_{k}^{T} = ((I - K_{k} C_{k}) ϵ_{k - 1} - K_{k} v_{k}) ((I - K_{k} C_{k}) ϵ_{k - 1} - K_{k} v_{k})^{T}$

$P_{k} = E (ϵ_{k} ϵ_{k}^{T})$ , and $P_{k - 1} = E (ϵ_{k - 1} ϵ_{k - 1}^{T})$ .

$E (ϵ_{k - 1} v_{k}^{T}) = E (ϵ_{k - 1}) E (v_{k}^{T}) = 0$ by the white noise property of $ϵ$ and $v$ . However, $E (v_{k} v_{k}^{T}) = R_{k}$ . Substituting all those into $P_{k}$ , we would get,

$P_{k} = (I - K_{k} C_{k}) P_{k - 1} (I - K_{k} C_{k})^{T} + K_{k} R_{k} K_{k}^{T}$

$P_{k} = P_{k - 1} - P_{k - 1} C_{k}^{T} K_{k}^{T} - K_{k} C_{k} P_{k - 1} + K_{k} C_{k} P_{k - 1} C_{k}^{T} K_{k}^{T} + K_{k} R_{k} K_{k}^{T}$

$W = t r (P_{k}) = t r (P_{k - 1}) - t r (P_{k - 1} C_{k}^{T} K_{k}^{T}) - t r (K_{k} C_{k} P_{k - 1}) + t r (K_{k} C_{k} P_{k - 1} C_{k}^{T} K_{k}^{T}) + t r (K_{k} R_{k} K_{k}^{T})$

We take F.O.C. to solve for $K_k = arg\min_{K_k} W_k = tr\bigg( \mathbb{E}(\epsilon_k \epsilon_k^T ) \bigg) = tr(P_k)$ , by letting $\frac{\partial W_k}{\partial K_k} = 0$ . See the Matrix Cookbook and find how to do derivatives w.r.t. $K_k$ .

$\frac{\partial W_{k}}{\partial K_{k}} = - 2 P_{k - 1} C_{k}^{T} + 2 K_{k} C_{k} P_{k - 1} C_{k}^{T} + 2 K_{k} R_{k} = 0$

We solve for $K_k$ ,

$K_{k} = P_{k - 1} C_{k}^{T} (R_{k} + C_{k} P_{k - 1} C_{k}^{T})^{- 1}$

, we let $L_k = R_k + C_k P_{k-1} C_k^T$ , and $L_k$ has the following property $L_k = L_k^T$ and $L_k^{-1} = (L_k^{-1})^T$

$K_{k} = P_{k - 1} C_{k}^{T} L_{k}^{- 1}$

Plug $K_k = P_{k-1} C_k^T K_k^{-1}$ back into $P_k$ .

$P_{k} = P_{k - 1} - K_{k} C_{k} P_{k - 1} = (I - K_{k} C_{k}) P_{k - 1}$

Summary

In the end, the Recursive Least Squared Method could be summarised as the following three equations.

1. Update the Gain Matrix.

$K_{k} = P_{k - 1} C_{k}^{T} (R_{k} + C_{k} P_{k - 1} C_{k}^{T})^{- 1}$

2. Update the Estimate.

${\hat{x}}_{k} = {\hat{x}}_{k - 1} + K_{k} (y_{k} - C_{k} {\hat{x}}_{k - 1})$

3. Propagation of the estimation error covariance matrix by using this equation.

$(I-K_k C_k)P_{k-1}$

ABOUT ME

Least Squares Method – Intro to Kalman Filter

Recursive Least Squared Method

Notation

Optimisation

Summary

Reference