卡尔曼滤波（三）—— 主体公式推导
公式推导
Xk=AXk-1+BUk+Wk-1
Zk = HXk + Vk
对于Wk-1 ，其概率分布 P(W) 服从(0,Q)，Q是协方差矩阵。
假设 X = [x1,x2]T，那么其中误差为w = [w1,w2]T, 其协方差为 Q =E(WWT)= E( [w1,w2]T [w1,w2] )。
同理：R = E(VVT) = E( [v1,v2]T [v1,v2] )
注：一般协方差为E( [W-E(W)]2 )，而这里没有减去E(W)，因为E(W)期望为0，高斯噪声。
算完里面，还要对每个元素求期望
*方差：Var(x) = E(x2) - E2(x)
例如：有x1、x2、x3
Var(x) = E(x2) - E2(x)
推算
最原始公式：
Var(X) = E( [X-E(X)]2 ) = 1/ n ∑(X-E(X))2
= 1 / n ∑ (X2 - 2X *E(x) + E2(X) ).......将X看作离散的值时
= E( X2 - 2XE(X) + E2(X) )
将E(X)看作一个普通的数
Var(X) = E( X2 ) - 2E(X)E(X) + E2(X) = E( X2 ) - E2(X)
1. X-k = AX^k-1 + BUk-1 .......(1)
Xk- 表示算出来的结果，也叫【先验结果】，不是【最优估计】，后边要将 【-】拿掉，变成Xk，一般地，在上面加上^，X^k，代表【后验结果】。
（而X^k-1 代表上一次的后验结果）
有的地方，用【Xk|k-1】代表【先验结果】，意思是用k-1时的值计算出k时的值；用【Xk|k】代表第k时的【后验结果】
2. Zk = HXMEA-k .......(2) 。 XMEA-k = H-Zk表示测出来的，因为Zk是测出来的
因为Wk-1 或 Vk 都是属于不可测，完全随机的，所以无论Xk- 、XMEA-k 都不可以直接使用。
3 . 最终：
X^k = X-k - G (H-Zk - X-k ).......(3) 。
（ Xk- 能算、Zk 能测，最后一步是算G了；当G等于零，Xk = Xk- 完全相信算出的结果；当G=1时，完全相信用G推出来的结果，也就是测出的结果）
4. 如果令 G = KkH , 上面写为：Xk = X-k - Kk (Zk - HX-k )，或 Xk|k=Xk|k-1 - Kk (Zk - HXk|k-1 )
根据G∈[0,1]，可以得出Kk ∈ [ 0, H-]
目标：寻找Kk，使得Xk^(后验结果)，最接近Xk（真实结果，但是不可知）
后验误差：ek = Xk - X^k ( 后验误差 = 真值 - 后验) ........(0)
* ek|k = Xk - Xk|k
误差的分布：P(ek) ~ [0, P]
P为【误差的协方差矩阵】：P = E(ekekT)
问题转化：
ek最小，其实就是tr(P) = σe12 + σe22 最小，tr()是求矩阵的迹。
【后验误差协方差矩阵】：
P = E(ekekT) = E[(Xk - X^k )(Xk - X^k )T] .......(1)
Xk - X^k = Xk - (X-k - G (H-Zk - X-k ))
= Xk - X-k - Kk Zk + Kk H X-k
= Xk - X-k - KkHXk - KkVk + Kk H X-k
= ( Xk - X-k) - KkH(Xk - X-k) - KkVk
= ( I - KkH)(Xk - X-k) - KkVk .......(2)
（或Xk - Xk|k = ( I - KkH)(Xk - Xk|k-1) - KkVk）
又有【先验误差】：
e-k = Xk - X-k (或 ek|k-1 =Xk - Xk|k-1）......(3)
（3） 代入（0）
Xk - X^k = ( I - KkH)e-k - KkVk ......(4)
（或 Xk - Xk|k = ( I - KkH)ek|k-1 - KkVk）
（4）代入（1）
P = E(ekekT) = E[(Xk - X^k )(Xk - X^k )T]
= E[(( I - KkH)e-k - KkVk )(( I - KkH)e-k - KkVk )T]
又 (AB)T =BTAT、(A+B)T= AT+BT
P = E [(( I - KkH)e-k - KkVk )(e-k T( I - KkH)T - VkTKkT )]
= E [ ( I - KkH)e-k e-k T( I - KkH)T - ( I - KkH)e-k VkTKkT - KkVk e-k T( I - KkH)T + KkVk VkTKkT]
* E(A+B+C) = E(A) + E(B) + E(C)
单独将( I - KkH)e-k VkTKkT 拿出来分析：
E(( I - KkH)e-kVkTKkT ) = ( I - KkH) E (e-k VkT) KkT
由于e-k 是【先验误差】，而 Vk 是【测量误差】，所以相互独立。
E (e-k VkT) = E (e-k ) E( VkT)，而Vk误差的期望为0，所以 E (e-k VkT) =0
所以：
P = E [ ( I - KkH)e-k e-k T( I - KkH)T - 0 - 0 + KkVk VkTKkT]
= ( I - KkH) E(e-k e-k T)( I - KkH)T + KkE(Vk VkT) KkT
E(e-k e-k T)其实就是【先验误差协方差矩阵】：
P-k = E(e-k e-k T)
P = ( P-k - KkHP-k) ( IT - HTKkT) + KkE(Vk VkT) KkT
又：R = E( [v1,v2]T [v1,v2] ) = E(VVT) = R （类似与水平角、HD、VD的协方差矩阵）
P = ( P-k - KkHP-k) ( IT - HTKkT) + KkRKkT
P = P-k - KkHP-k - P-k HTKkT + KkHP-k HTKkT + KkRKkT
（由于P-k = E(e-k e-k T) = P-kT ，KkHP-k 和 P-k HTKkT互为转至，所以痕是一样的）
目标：寻找Kk，使得， tr(P) = σe12 + σe22 最小
tr(P) = tr( P-k) - 2 * tr( KkHP-k) + tr(KkHP-k HTKkT)+ tr(KkRKkT)
那么，对上式对Kk求导，可得：
dtr(P) / dKk = 0
第一项：tr( P-k)/ dKk = 0 （没有Kk，所以为0）
第二项 ：tr( KkHP-k)/ dKk
----------------------------------------------------------------
补充知识，矩阵求导：
d tr(AB) / dA = BT
进而，有d tr(ABAT) / dA =2AB
-----------------------------------------------------------------
第二项 ：tr( KkHP-k)/ dKk = 2(HP-k)T
第三项：tr(KkHP-k HTKkT) / dKk= 2 KkHP-kHT
第四项：tr(KkRKkT)/ dKk = 2 KkR
dtr(P) / dKk = 0 - 2(HP-k)T + 2 KkHP-kHT + 2 KkR = 0
整理：
dtr(P) / dKk = (HP-k)T + KkHP-kHT + KkR = 0
dtr(P) / dKk = P-kTHT + KkHP-kHT + KkR = 0
（P-k是协方差矩阵，对称）
dtr(P) / dKk = P-kHT + Kk(HP-kHT + R) = 0
Kk = P-kHT / (HP-kHT + R)
(R特别大，Kk接近0，那么Xk = X-k - Kk (Zk - HX-k ) ≈ X-k ，接近模型算出的)
(R特别小，Kk接近H-，那么Xk ≈ H- Zk ，接近由观测值算出的X)
遗留的问题：
P-k = E(e-k e-k T)
e-k = Xk - X-k
Xk作为真值不可知，怎样求？
猜测：因为Xk作为真值，没有误差，那么实际上就是X-k 的协方差，
又X-k = AX^k-1 + BUk-1 , 所以实际上又和k-1时的【后验协方差矩阵】相关
