优化问题的一般思路
有观测值的协方差矩阵—— 使用高斯分布
先参考：(3 封私信 / 72 条消息) 多维高斯分布是如何由一维发展而来的？ - 知乎 (zhihu.com)
假设有已经线性化的观测方程：
L + V = f(X) = BX + d ，L是观测值向量，v是误差，z服从高斯分布， L ~ N ( μ , ∑)，∑为协方差矩阵
（B是模型 L = f(X) ，在X0处求导的矩阵， 注意，在卡尔曼滤波，是Z = AX + V，但是观测值有误差，应该是 Z + V = AX）
可以看到：
X = X0 + ΔX
L = BX + d - V
L = B(X0 + ΔX ) + d - V
L = BX0 + BΔX + d - V
又【模型观测值】 L0 = BX0 + d
因此：
L = L0 + BΔX - V
V = BΔX - ( L - L0)
令：l = L - L0
那么：
V = BΔX - l
-----------------------------------------------------------------------------------------
令一种思路：
L + V = f(X) // 观测值 + 误差 = f(X)
f(X)进行泰勒一阶展开
L + V = f(X0) + f ' (X0) * (X - X0)
ΔX = X - X0
L + V = f(X0) + BΔX
又 L0 = f(X0)
L + V = L0 + BΔX
因此：
V = BΔX - ( L - L0)
令：l = L - L0
那么：
V = BΔX - l
-----------------------------------------------------------------------------------------
L 的高斯分布为：
P = ∑-1
P(z) = 1 / (2Π * | ∑| 1/2 ) * exp ( - 1 / 2 * ( L - μ )T * P * ( L - μ ) )
令：
假设x是最有可能的值，因为μ也是最有可能的值，那么 μ = f(X)
P(z) = 1 / (2Π * | ∑| 1/2 ) * exp ( - 1 / 2 * (- V)T * P * (- V)) = 1 / (2Π * | ∑| 1/2 ) * exp ( - 1 / 2 * VT * P * V)
问题是，求得：
x = argmax P(L)
相当于求：
x = argmax (- 1 / 2 * VT * P * V )
相当于求：
x = argmin ( VT * P * V )
然后对 ∂ VT * P * V / ∂ΔX = 2VT * P * ∂V / ∂ΔX = 2VT * P * B = 0
相当于：
BT * P * V = 0
BT * P * ( BΔX - l) = 0
BT * P* B* ΔX = BT * P * l
A =BT * P* B*
b =BT * P * l
AΔX = b
X = X0 + ΔX
无观测值的协方差矩阵
Li + Vi = f(X)
线性化：
Vi = AiX - Li
Vi = AiX0 + AiΔX - Li
令：fx = AiX0 - Li = L0i - Li = - li
Vi = fx + AiΔX
不使用迭代就直接求X也行
求：
X = argmin ∑ 1 / 2 || Vi ||²
= argmin ∑ 1 / 2 Vi TVi
= argmin ∑ 1 / 2 ( fx + AiΔX )T( fx + AiΔX )
= argmin ∑ 1 / 2 (fxTfx + fxT AiΔX + ΔXTAiTfx + ΔXTAiTAiΔX)
= argmin ∑ 1 / 2 (fxTfx + 2 * fxT AiΔX + ΔXTAiTAiΔX)
对ΔX求导
∑ Ai Tfx + AiTAiΔX = 0
∑AiTAiΔX = - ∑ Ai fx
∑AiTAiΔX = ∑ Ai Tli
-----------------------------------------------------------------------------
或者所有V都写在一条式子中
V = AX - L
X = argmin 1 / 2 || V ||²
直接推算得到
V = AX - L
X = argmin 1 / 2 VTV
若可以直接凑出形如：
X = argmin 1 / 2 * C * （X - Xpi）
那当 X = Xpi时，能得到 1 / 2 VTV 最小，直接解出X
