Kalman Filter to minimize weighted errors on the states: what's wrong with my derivation

I am thinking about how to implement a "weighted Kalman Filter". Note that the weights here are on the states. Basically the classical KF minimizes $$\sum (x_i - \hat{x_i} )^2$$ but I want to minimize $$\sum w_i(x_i - \hat{x_i} )^2$$ where $$w_i$$ is an externally supplied vector.

Citing wikipedia derivation

update function is

\begin{aligned} \mathbf {P} _{k\mid k-1}&=\mathbf {F} _{k}\mathbf {P} _{k-1\mid k-1}\mathbf {F} _{k}^{\textsf {T}}+\mathbf {Q} _{k},\\ \mathbf {S} _{k}&=\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}+\mathbf {R} _{k},\\ \mathbf {K} _{k}&=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {S} _{k}^{-1},\\ \mathbf {P} _{k|k}&=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k|k-1}. \end{aligned}

How do we find the optimal Kalman gain? We take derivative of $$P_{k|k}$$ over $$K$$

\begin{align} \mathbf{P}_{k\mid k} & = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k\right) \mathbf{K}_k^\textsf{T} \\&= \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k\mathbf{K}_k^\textsf{T} \end{align}

$$\frac{\partial \; \operatorname{tr}(\mathbf{P}_{k\mid k})}{\partial \;\mathbf{K}_k} = -2 \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\right)^\textsf{T} + 2 \mathbf{K}_k \mathbf{S}_k = 0.$$

\begin{align} \mathbf{K}_k \mathbf{S}_k &= \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\right)^\textsf{T} = \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \\ \Rightarrow \mathbf{K}_k &= \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1} \end{align}

However this is an "unweighted" Kalman Filter. We minimize the trace which is the sum of squared errors. But what if we want to minimize a "weighted" version of the squared errors? I want to make sure the errors in estimating some states carry more weights than others... We can assume we know what the weights are

My Attempts

Since trace of matrix P is the objective function to optimize, we want to somehow create an expression $$P^{'}_{k|k} = diag(w) \cdot P_{k|k} \cdot diag(w)$$, where $$w$$ is a vector of weights with length equal to the number of states. Then we take derivative of $$P^{'}_{k|k}$$ to $$K$$.

Given $$\mathbf{P}_{k\mid k} = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k\mathbf{K}_k^\textsf{T}$$

We can multiply $$W = diag(w)$$ on both ends:

$$tr (WP_{k|k}W^T) = tr(P_{k|k} W W^T ) = tr(P_{k|k}\Omega )$$ (since trace of matrix product is communicative. Here we let $$W W^T = \Omega$$ ).

\begin{align} \frac{\partial tr(\mathbf {P} _{k\mid k}\Omega)}{\partial \mathbf{K}_k} &= \frac{\partial tr(\mathbf{P}_{k\mid k-1}\Omega - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1}\Omega - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T}\Omega + \mathbf{K}_k \mathbf{S}_k\mathbf{K}_k^\textsf{T}\Omega )}{\partial \mathbf{K}_k} \\ &= -2(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\Omega)^T+2\Omega \mathbf{K}_k \mathbf{S}_k \\ &= 0 \end{align}

Therefore

$$\Omega \mathbf{K}_k \mathbf{S}_k = (\mathbf{H}_k \mathbf{P}_{k\mid k-1}\Omega)^T$$

Since $$\Omega$$ and $$P$$ are symmetrical

$$\Omega \mathbf{K}_k \mathbf{S}_k = \Omega \mathbf{P}_{k\mid k-1} \mathbf{H}^T_k$$

And $$\Omega$$ would be canceled out...It seems like Kalman gain is the same with or without weights? That seems wrong ?

• $\Omega$ cannot simply be factored out and cancelled when deriving the weighted gain. Commented Jun 24 at 12:06
• @RobertLong why though? I added just one last line, $\Omega$ and $P$ are symmetrical? Commented Jun 24 at 12:10
• Introducing weights $\Omega$ fundamentally I think changes the optimisation problem. In the classical Kalman filter, we minimise the trace of the error covariance matrix, which is invariant under transposition. When weights are introduced, the objective function becomes $tr(P_{k|k}\Omega)$ which is not the same as minimising the unweighted trace. Commented Jun 24 at 12:31

TL;DR: I think the mistake lies in the incorrect assumption that the weighting matrix $$\Omega$$ can be factored out and canceled in the optimisation. Rather, it should show how the weights directly impact the Kalman gain calculation.

Derivation of a Weighted Kalman Filter

To derive a "weighted Kalman Filter" where the weights are on the states, the key is to correctly incorporate the weighting into the objective function and then derive the optimal Kalman gain.

1. Review of the Classical Kalman Filter
The classical Kalman Filter minimises the unweighted sum of squared errors: $$\sum (x_i - \hat{x_i})^2$$. The update equations are: \begin{align*} \mathbf{P}_{k\mid k-1} &= \mathbf{F}_k \mathbf{P}_{k-1\mid k-1} \mathbf{F}_k^\textsf{T} + \mathbf{Q}_k, \\ \mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k, \\ \mathbf{K}_k &= \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1}, \\ \mathbf{P}_{k\mid k} &= (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\mid k-1}. \end{align*}

2. Weighted Error Minimisation
The objective function for the weighted Kalman Filter is to minimise $$\sum w_i (x_i - \hat{x_i})^2$$. This can be expressed in matrix form as $$\text{tr}(\mathbf{W} \mathbf{P}_{k\mid k} \mathbf{W})$$, where $$\mathbf{W}$$ is a diagonal matrix of weights.

3. Modify the Error Covariance Matrix
Given $$\mathbf{P}_{k\mid k} = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^\textsf{T},$$ we need to incorporate the weights into this covariance matrix.

4. Derive the Weighted Kalman Gain
Minimise the trace of the weighted error covariance matrix: $$\text{tr}(\mathbf{W} \mathbf{P}_{k\mid k} \mathbf{W})$$. Take the derivative: \begin{align*} \frac{\partial \text{tr}(\mathbf{W} \mathbf{P}_{k\mid k} \mathbf{W})}{\partial \mathbf{K}_k} &= \frac{\partial \text{tr}(\mathbf{W} (\mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^\textsf{T}) \mathbf{W})}{\partial \mathbf{K}_k} \\ &= -2 (\mathbf{W} \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W})^\textsf{T} + 2 (\mathbf{W} \mathbf{K}_k \mathbf{S}_k \mathbf{W}) = 0. \end{align*} Solve for $$\mathbf{K}_k$$: \begin{align*} \mathbf{W} \mathbf{K}_k \mathbf{S}_k \mathbf{W} &= (\mathbf{W} \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W})^\textsf{T}, \\ \Rightarrow \mathbf{K}_k \mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W}^2, \\ \Rightarrow \mathbf{K}_k &= (\mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W}^2) \mathbf{S}_k^{-1}. \end{align*}

5. Verify Correctness
Verify if the weights correctly influence the Kalman gain and ensure the final gain incorporates the weights appropriately. Specifically, when weights are uniform (i.e., $$\mathbf{W} = \mathbf{I}$$), the solution should reduce to the classical Kalman Filter gain: $$\mathbf{K}_k = \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1}.$$ In the weighted case, the Kalman gain becomes: $$\mathbf{K}_k = (\mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W}^2) \mathbf{S}_k^{-1},$$ where $$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k$$ remains unchanged. The updated state estimate and error covariance matrix then incorporate the weighted Kalman gain:

\begin{align} \mathbf{x}_{k\mid k} &= \mathbf{x}_{k\mid k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{x}_{k\mid k-1}), \\ \mathbf{P}_{k\mid k} &= (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\mid k-1}. \end{align}

This ensures that the weights correctly influence the error minimisation objective, reflecting their intended effect on the state error covariance matrix.

• Thanks for the detailed answer. Why is $tr (WP_{k|k}W^T) = tr(P_{k|k} W W^T ) = tr(P_{k|k}\Omega )$ not true though? The trace of matrix product is communicative? Commented Jun 24 at 13:25
• Can you also explain why \begin{align*} \mathbf{W} \mathbf{K}_k \mathbf{S}_k \mathbf{W} &= (\mathbf{W} \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W})^\textsf{T}, \\ \Rightarrow \mathbf{K}_k \mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{W}^2 \end{align*} Commented Jun 24 at 13:26
• I'm on a bus right now with only my phone so it's a bit difficult to give you detailed steps, but for your first comment, I think the first expression can be true under certain conditions, eg. if $\Omega$ is defined as $\mathbf{W} \mathbf{W}$. This shouldn't affect my answer above. Commented Jun 24 at 16:11
• For your 2nd comment, evaluate the transpose on the RHS; Use the fact that matrices in the resulting equation are symmetric, meaning they remain the same when transposed; substitute back into the original equation, isolate $K_k$ by premultiplying and postmultiplying both sides by $W^{-1}$ (obviously assuming invertibility); finally simplify the resulting equation to get the desired result. Commented Jun 24 at 16:11
• Let me know if you need to see all the steps in detail, and I'll have a go at that a bit later. Commented Jun 24 at 16:12