Maximum A Posteriori

The goal of MAP is generally:

$$\hat{\mathbf{x}}=\underset{\mathbf{x}}{\mathrm{argmax}}p(\mathbf{x}|\mathbf{v},\mathbf{y})$$ $$\mathbf{x}={\mathbf{x}}_{0:K},\mathbf{v}=({\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:K}),\mathbf{y}={\mathbf{y}}_{0:K}$$

Which means that we are trying to find the single best estimate for the state of the system given our control input and measurements.

Using Bayes’ Theorem

$$\hat{\mathbf{x}}=\underset{\mathbf{x}}{\mathrm{argmax}}p(\mathbf{x}|\mathbf{v},\mathbf{y})=\underset{\mathbf{x}}{\mathrm{argmax}}\frac{p(\mathbf{y}|\mathbf{x},\mathbf{v})p(\mathbf{x}|\mathbf{v})}{p(\mathbf{y}|\mathbf{v})}=\underset{\mathbf{x}}{\mathrm{argmax}}p(\mathbf{y}|\mathbf{x})p(\mathbf{x}|\mathbf{v})$$

yes you can single out specific variables and keep the givens, here was a singling out $x$ and $y$ and how they interact with each other

We can drop the denominator because it doesn’t depend on $\mathbf{x}$ (we are trying to argmax here). We can drop $\mathbf{v}$ because $\mathbf{y}$ doesn’t depend on it. (see observation model in LG Problem Statement).

Each set of state and measurement is independent of the other sets, so

$$p(\mathbf{y}|\mathbf{x})=\prod _{k=0}^{K}p({\mathbf{y}}_k|{\mathbf{x}}_k)$$

Looking at our motion model

$$\text{model:}{\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k$$

we see that ${\mathbf{x}}_k$ depends on its previous state and the input. As a result, we can factor $\mathbf{p}(\mathbf{x}|\mathbf{v})$ as

$$p(\mathbf{x}|\mathbf{v})=p({\mathbf{x}}_0|{\check{\mathbf{x}}}_0)\prod _{k=1}^{K}p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)$$

Which gives us

$$\underset{\mathbf{x}}{\mathrm{argmax}}p(\mathbf{y}|\mathbf{x})p(\mathbf{x}|\mathbf{v})=\underset{\mathbf{x}}{\mathrm{argmax}}p({\mathbf{x}}_0|{\check{\mathbf{x}}}_0)\prod _{k=0}^{K}p({\mathbf{y}}_k|{\mathbf{x}}_k)\prod _{k=1}^{K}p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)$$

From the LG Problem Statement its its motion and observation models, we can get that:

$$p({\mathbf{x}}_0|\check{{\mathbf{x}}_0})\sim \mathcal{N}({\check{\mathbf{x}}}_0,{\check{\mathbf{P}}}_0)$$ $$p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)\sim \mathcal{N}({\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k,{\mathbf{Q}}_k)$$ $$p({\mathbf{y}}_k|{\mathbf{x}}_k)\sim \mathcal{N}({\mathbf{C}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)$$

To make optimization easier, the logarithm of both sides is taken

$$\underset{\mathbf{x}}{\mathrm{argmax}}\ln (p(\mathbf{y}|\mathbf{x})p(\mathbf{x}|\mathbf{v}))=\underset{\mathbf{x}}{\mathrm{argmax}}\ln p({\mathbf{x}}_0|{\check{\mathbf{x}}}_0)+\sum _{k=1}^K\ln p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)+\sum _{k=0}^K\ln p({\mathbf{y}}_k|{\mathbf{x}}_k)$$

where

$$\ln p({\mathbf{x}}_0|{\check{\mathbf{x}}}_0)=-\frac{1}{2}({\mathbf{x}}_0-{\check{\mathbf{x}}}_0{)}^T{\mathbf{P}}_0^{-1}({\mathbf{x}}_0-{\check{\mathbf{x}}}_0)\underset{\text{independant of x}}{\underbrace{-\text{cancelto}0\frac{1}{2}\ln ((2\pi {)}^N\det {\mathbf{P}}_0)}}$$ $$\ln p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)=-\frac{1}{2}({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k{)}^T{\mathbf{Q}}_k^{-1}({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k)-\underset{\text{independant of x}}{\underbrace{\text{cancelto}0\frac{1}{2}\ln ((2\pi {)}^N\det {\mathbf{Q}}_k)}}$$ $$\ln p({\mathbf{y}}_k|{\mathbf{x}}_k)=-\frac{1}{2}({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k{)}^T{\mathbf{R}}_k^{-1}({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k)-\underset{\text{independant of x}}{\underbrace{\text{cancelto}0\frac{1}{2}\ln ((2\pi {)}^M\det {\mathbf{R}}_k)}}$$

After cancelling the terms that are independent of x, we can construct an objective function.

$$J(\mathbf{x})=\sum _{k=0}^K(J_{v,k}(\mathbf{x})+J_{y,k}(\mathbf{x}))$$ $$J_{v,k}(\mathbf{x})=\{\begin{array}{ll}\frac{1}{2}({\mathbf{x}}_0-{\check{\mathbf{x}}}_0{)}^T{\mathbf{P}}_0^{-1}({\mathbf{x}}_0-{\check{\mathbf{x}}}_0)& k=0\\ \frac{1}{2}({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k{)}^T{\mathbf{Q}}_k^{-1}({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k)& k=1\dots K\end{array}$$ $$J_{y,k}(\mathbf{x})=\frac{1}{2}({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k{)}^T{\mathbf{R}}_k^{-1}({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k)$$

This objective function is directly grabbed from the logged gaussians you derived above. The negative has been removed so it becomes a minimization problem

So we end up with a simplified, equivalent optimization problem

$$\hat{\mathbf{x}}=\underset{\mathbf{x}}{\mathrm{argmin}}J(\mathbf{x})$$

We can convert our objective function into a cleaner Matrix Form by cleverly stacking our known data.

$$\mathbf{z}=\left[\begin{array}{c}{\mathbf{x}}_0\\ {\mathbf{v}}_1\\ ⋮\\ {\mathbf{v}}_K\\ {\mathbf{y}}_0\\ {\mathbf{y}}_1\\ ⋮\\ {\mathbf{y}}_K\end{array}\right],\mathbf{x}=\left[\begin{array}{c}{\mathbf{x}}_0\\ ⋮\\ {\mathbf{x}}_K\end{array}\right]$$

We then define the following block-matrix quantities:

$$\mathbf{H}=\left[\begin{array}{cccc}1& & & \\ -{\mathbf{A}}_0& 1& & \\ & \ddots & \ddots & \\ & & -{\mathbf{A}}_{K-1}& 1\\ {\mathbf{C}}_0& & & \\ & {\mathbf{C}}_1& & \\ & & \ddots & \\ & & & {\mathbf{C}}_K\end{array}\right]$$ $$\mathbf{W}=\left[\begin{array}{cccccccc}{\mathbf{P}}_0& & & & & & & \\ & {\mathbf{Q}}_1& & & & & & \\ & & \ddots & & & & & \\ & & & {\mathbf{Q}}_K& & & & \\ & & & & {\mathbf{R}}_0& & & \\ & & & & & {\mathbf{R}}_1& & \\ & & & & & & \ddots & \\ & & & & & & & {\mathbf{R}}_K\end{array}\right]$$

This lets us represent our objective function as:

$$J(\mathbf{x})=\frac{1}{2}(\mathbf{z}-\mathbf{Hx}{)}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{Hx})$$

Since $J(\mathbf{x})$ is a paraboloid, there exists a closed form solution if we set its partial derivative with respect to $\mathbf{x}$ to 0.

$$\frac{\partial J(\mathbf{x})}{\partial {\mathbf{x}}^T}{|}_{\hat{\mathbf{x}}}=0$$ $$-{\mathbf{H}}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{H}\hat{\mathbf{x}})-0$$ $$({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H})\hat{\mathbf{x}}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{z}$$

Which is a Normal Equation!

It is important to note here that Maximum A Posteriori will optimize to the mode of the distribution!