Finding Covariance W

From Maximum A Posteriori we have the loss function:

$$J(\mathbf{x})=\frac{1}{2}\sum _{i=1}^N{\mathbf{e}}_i(\mathbf{x}{)}^T{\mathbf{W}}_i^{-1}({\mathbf{e}}_i(\mathbf{x}))$$

But how do we set ${\mathbf{W}}_i$?

Previously we were assuming that ${\mathbf{W}}_{v,k}$ and ${\mathbf{W}}_{y,k}$ can be thought of as positive-definite symmetric matrix weights that are often set to the process noise and measurement noise covariances of the system

So

$${\mathbf{W}}_{v,k}={\mathbf{Q}}_k{\mathbf{W}}_{y,k}={\mathbf{R}}_k$$ $$\mathbf{W}=\text{diag}({\mathbf{W}}_v,{\mathbf{W}}_y),{\mathbf{W}}_v=\text{diag}({\mathbf{W}}_{v,0},\dots ,{\mathbf{W}}_{v,K})$$ $${\mathbf{W}}_y=\text{diag}({\mathbf{W}}_{y,0},\dots ,{\mathbf{W}}_{y,K})$$

But this assumes that we know the process noise and measurement noise beforehand.

We could use a datasheet, but that often isn't reliable

Supervised Covariance Estimation

Given we have a set of $K$ groundtruth state values, ${\mathbf{x}}_{true}$. We can compute the process noise and measurement noise as:

$$\mathbf{Q}=\frac{1}{K-1}\sum _{k=1}^K({\mathbf{e}}_{v,k}({\mathbf{x}}_{true})-{\bar{\mathbf{e}}}_v)({\mathbf{e}}_{v,k}({\mathbf{x}}_{true})-{\bar{\mathbf{e}}}_v{)}^T$$ $$\mathbf{R}=\frac{1}{K-1}\sum _{k=1}^K({\mathbf{e}}_{y,k}({\mathbf{x}}_{true})-{\bar{\mathbf{e}}}_v)({\mathbf{e}}_{y,k}({\mathbf{x}}_{true})-{\bar{\mathbf{e}}}_v{)}^T$$ $${\bar{\mathbf{e}}}_v=\frac{1}{K}\sum _{k=1}^K{\mathbf{e}}_{v,k}({\mathbf{x}}_{true}){\bar{\mathbf{e}}}_y=\frac{1}{K+1}\sum _{k=1}^K{\mathbf{e}}_{y,k}({\mathbf{x}}_{true})$$

where from Maximum A Posteriori

$${\mathbf{e}}_{v,k}(\mathbf{x})=\{\begin{array}{ll}{\check{\mathbf{x}}}_0-{\mathbf{x}}_0& k=0\\ \mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{v}}_k,0)& k=1\dots K\end{array}$$

\mathbf{e}{y,k}(\mathbf{x})=\mathbf{y}{k}-\mathbf{g}(\mathbf{x}_{k},\mathbf{0});; k=0\dots K

Once we have characterized the noise, we can proceed to using them in a real operational scenario. # Adaptive Covariance Estimation Using a trailing window of $L$ datapoints from the current time, $k$, we can adaptively change our covariances according to **Measurement Noise Covariance**

\bar{\mathbf{e}}{y,k} = \frac{1}{L} \sum{\ell=k-1}^{k-L} \mathbf{e}_{y,\ell}

\mathbf{S}{y,k} = \frac{1}{L-1} \sum{\ell=k-1}^{k-L} (\mathbf{e}{y,\ell} - \bar{\mathbf{e}}{y,k})(\mathbf{e}{y,\ell} - \bar{\mathbf{e}}{y,k})

\mathbf{R}k = \mathbf{S}{y,k} - \frac{1}{L} \sum_{\ell=k-1}^{k-L} \mathbf{G}\ell \mathbf{P}\ell \mathbf{G}_\ell

$$\ast \ast ProcessNoiseCovariance\ast \ast$$

\bar{\mathbf{e}}{v,k} = \frac{1}{L} \sum{\ell=k-1}^{k-L} \mathbf{e}_{v,\ell}

\mathbf{S}{v,k} = \frac{1}{L-1} \sum{\ell=k-1}^{k-L} (\mathbf{e}{v,\ell} - \bar{\mathbf{e}}{v,k})(\mathbf{e}{v,\ell} - \bar{\mathbf{e}}{v,k})

\mathbf{Q}k = \mathbf{S}{v,k} - \frac{1}{L} \sum_{\ell=k-1}^{k-L} \mathbf{F}{\ell-1} \mathbf{P}{\ell-1} \mathbf{F}_{\ell-1}