最大后验估计MAP
x ^ = arg max x p ( x ∣ u , y ) \hat{x}=\arg \max _{x}p\left( x|u,y\right) x^=argxmaxp(x∣u,y)
我们希望在给定先验信息和所有时刻的输入 u u u和观测 y y y，推断出所有时刻的最优状态 x ^ \hat{x} x^。为此我们定义几个宏观变量。
x = x 0 : K = ( x 0 , ⋯ , x K ) u = ( x ˇ 0 , u 1 : k ) = ( x ˇ 0 , u 1 , ⋯ , u K ) y = y 0 : K = ( y 0 , ⋯ , y K ) \begin{aligned} x&=x_{0:K}=\left( x_{0},\cdots ,x_{K}\right) \\ u&=(\check{x}_{0},u_{1:k}) =\left( \check{x}_{0},u_{1},\cdots ,u_{K}\right)\\ y&=y_{0:K}=\left( y_{0},\cdots ,y_{K}\right) \end{aligned} xuy=x0:K=(x0,⋯,xK)=(xˇ0,u1:k)=(xˇ0,u1,⋯,uK)=y0:K=(y0,⋯,yK)
用贝叶斯公式重写MAP估计：
x ^ = arg max x p ( x ∣ u , y ) = arg max x p ( y ∣ x , u ) p ( x ∣ u ) p ( y ∣ u ) = arg max x p ( y ∣ x ) p ( x ∣ u ) \hat{x}=\arg \max _{x}p\left( x| u,y\right) =\arg \max _{x}\dfrac{p\left( y|x,u\right) p\left( x|u\right) }{p\left( y| u\right) }=\arg \max _{x}p\left( y| x\right) p\left( x| u\right) x^=argxmaxp(x∣u,y)=argxmaxp(y∣u)p(y∣x,u)p(x∣u)=argxmaxp(y∣x)p(x∣u)
这里我们吧分母略去，因为它与 x x x无关。同时省略 p ( y ∣ x , u ) p(y|x,u) p(y∣x,u)中的 u u u，因为如果 x x x已知，它不会影响观测数据(观测方程与它无关)。
接下来我们做出一个重要假设：对于所有时刻 k = 0 , ⋯ , K k=0,\cdots,K k=0,⋯,K，所有噪声项 w k w_k wk和 n k n_k nk之间是无关的，即 y k y_k yk只与 x k x_k xk有关，则可以用乘法公式对 p ( y ∣ x ) p(y|x) p(y∣x)进行因子分解：
p ( y ∣ x ) = p ( y 0 ∣ x 0 : K ) p ( y 1 ∣ x 0 : K , y 0 ) ⋯ p ( y k ∣ x 0 : K , y 0 : K − 1 ) = ∏ k = 0 K p ( y k ∣ x k ) \begin{aligned} p\left( y| x\right) &=p\left( y_{0}| x_{0:K}\right) p\left( y_{1}| x_{0:K},y_{0}\right) \cdots p\left( y_{k}| x_{0:K},y_{0:K-1}\right) \\ &=\prod ^{K}_{k=0}p\left( y_{k}| x_{k}\right) \end{aligned} p(y∣x)=p(y0∣x0:K)p(y1∣x0:K,y0)⋯p(yk∣x0:K,y0:K−1)=k=0∏Kp(yk∣xk)
同理， x k x_k xk只与 x k − 1 , u k x_{k-1},u_k xk−1,uk 有关，可得：
p ( x ∣ u ) = p ( x 0 ∣ u ) p ( x 1 ∣ u , x 0 ) … p ( x K ∣ u , x 0 : K − 1 ) = p ( x 0 ∣ x ˇ 0 ) ∏ k = 1 K p ( x k ∣ x k − 1 , u k ) \begin{aligned} p\left( x| u\right) &=p\left( x_{0}| u\right) p\left( x_{1}| u,x_{0}\right) \ldots p\left( x_{K}| u,x_{0:K-1}\right) \\ &=p\left( x_{0}| \check{x}_{0}\right) \prod ^{K}_{k=1}p\left( x_{k}| x_{k-1},u_{k}\right) \end{aligned} p(x∣u)=p(x0∣u)p(x1∣u,x0)…p(xK∣u,x0:K−1)=p(x0∣xˇ0)k=1∏Kp(xk∣xk−1,uk)
p ( x 0 ∣ x ˇ 0 ) = 1 ( 2 π ) N det P ˇ 0 × exp ( − 1 2 ( x 0 − x ˇ 0 ) T P ˇ 0 − 1 ( x 0 − x ˇ 0 ) ) p\left( x_{0}| \check{x}_{0}\right) =\dfrac{1}{\sqrt{\left( 2\pi \right) ^{N}\det \check{P}_{0}}}\times \exp \left( -\dfrac{1}{2}\left( x_{0}-\check{x}_{0}\right) ^{T}\check{P}_{0}^{-1}\left( x_{0}-\check{x}_{0}\right) \right) p(x0∣xˇ0)=(2π)NdetPˇ01×exp(−21(x0−xˇ0)TPˇ0−1(x0−xˇ0))
p ( x k ∣ x k − 1 , u k ) = 1 ( 2 π ) N det Q k × exp ( − 1 2 ( x k − f ( x k − 1 , u k , 0 ) ) T Q k − 1 ( x k − f ( x k − 1 , u k , 0 ) ) ) p\left( x_{k}| x_{k-1},u_{k}\right) =\dfrac{1}{\sqrt{\left( 2\pi \right) ^{N}\det Q_{k}}}\times \exp \left( -\dfrac{1}{2}\left( x_{k}-f\left( x_{k-1},u_{k},0\right) \right) ^{T}Q_{k}^{-1}\left( x_{k}-f\left( x_{k-1},u_{k},0\right) \right) \right) p(xk∣xk−1,uk)=(2π)NdetQk1×exp(−21(xk−f(xk−1,uk,0))TQk−1(xk−f(xk−1,uk,0)))
p ( y k ∣ x k ) = 1 ( 2 π ) M det R k × exp ( − 1 2 ( y k − h ( x k , 0 ) ) T R k − 1 ( y k − h ( x k , 0 ) ) ) p\left( y_{k}| x_{k}\right) =\dfrac{1}{\sqrt{\left( 2\pi \right) ^{M}\det R_{k}}}\times \exp \left( -\dfrac{1}{2}\left( y_{k}-h\left( x_{k},0\right) \right) ^{T}R_{k}^{-1}\left( y_{k}-h\left( x_{k},0\right) \right) \right) p(yk∣xk)=(2π)MdetRk1×exp(−21(yk−h(xk,0))TRk−1(yk−h(xk,0)))
对等式两侧取对数：
ln ( p ( y ∣ x ) p ( x ∣ u ) ) = ln p ( x 0 ∣ x ˇ 0 ) + ∑ k = 1 k ln p ( x k ∣ x k − 1 , u k ) + ∑ k = 0 k ln p ( y k ∣ x k ) \ln \left( p\left( y| x\right) p\left( x|u\right) \right) =\ln p\left( x_{0}| \check{x}_{0}\right) +\sum ^{k}_{k=1}\ln p\left( x_{k}| x_{k-1},u_{k}\right) +\sum ^{k}_{k=0}\ln p\left( y_{k}| x_{k}\right) ln(p(y∣x)p(x∣u))=lnp(x0∣xˇ0)+k=1∑klnp(xk∣xk−1,uk)+k=0∑klnp(yk∣xk)
ln p ( x 0 ∣ x ˇ 0 ) = − 1 2 ( x 0 − x ˇ 0 ) T P 0 − 1 ( x 0 − x ˇ 0 ) − 1 2 ln ( ( 2 π ) N det P ˇ 0 ) ⏟ 与 x 无关 \ln p\left( x_{0}| \check{x}_{0}\right) =-\dfrac{1}{2}\left( x_{0}-\check{x}_{0}\right) ^{T}P_{0}^{-1}\left( x_{0}-\check{x}_{0}\right) -\underbrace{\dfrac{1}{2}\ln \left( \left( 2\pi \right) ^{N}\det \check{P}_{0}\right)}_{与x无关} lnp(x0∣xˇ0)=−21(x0−xˇ0)TP0−1(x0−xˇ0)−与x无关 21ln((2π)NdetPˇ0)
ln p ( x k ∣ x k − 1 , u k ) = − 1 2 ( x k − f ( x k − 1 , u k , 0 ) ) T Q k − 1 ( x k − f ( x k − 1 , u k , 0 ) ) − 1 2 ln ( ( 2 π ) N det Q k ) ⏟ 与 x 无关 \ln p\left( x_{k}| x_{k-1},u_{k}\right) =-\dfrac{1}{2}\left( x_{k}-f\left( x_{k-1},u_{k},0\right) \right) ^{T}Q_{k}^{-1}\left( x_{k}-f\left( x_{k-1},u_{k},0\right) \right) -\underbrace{\dfrac{1}{2}\ln \left( \left( 2\pi \right) ^{N}\det Q_{k}\right)}_{与x无关} lnp(xk∣xk−1,uk)=−21(xk−f(xk−1,uk,0))TQk−1(xk−f(xk−1,uk,0))−与x无关 21ln((2π)NdetQk)
ln p ( y k ∣ x k ) = − 1 2 ( y k − h ( x k , 0 ) ) π R k − 1 ( y k − h ( x k , 0 ) ) − 1 2 ln ( ( 2 π ) M det R k ) ⏟ 与 x 无关 \ln p\left( y _{k}| x_{k}\right) =-\dfrac{1}{2}\left( y_{k}-h\left( x_{k},0\right) \right) ^{\pi }R_{k}^{-1}\left( y_{k}-h\left( x_{k},0\right) \right) -\underbrace{\dfrac{1}{2}\ln \left( \left( 2\pi \right) ^{M}\det R_{k}\right)}_{与x无关} lnp(yk∣xk)=−21(yk−h(xk,0))πRk−1(yk−h(xk,0))−与x无关 21ln((2π)MdetRk)
)
