奇异值鲁棒的卡尔曼滤波

经典卡尔曼滤波的主要公式如下：

其中，$Q, R$是表征状态噪声和观测噪声的协方差，都是基于高斯分布的假设。卡尔曼的输出是观测与预测的结合。$Q,R$就代表了滤波器对观测和预测分别的不信任程度。如果观测噪声较大，那么R比较大，系统输出就偏向预测值，否则偏向观测值。

$Q$和$R$的选取可以利用EM算法（详见页面最下方），通过最大化对数似然函数学习这些参量。对数似然函数为：

EM算法顾名思义分为Expectation步、Maximization步。对于一个估计参数问题，算法首先给定参数的初值，然后计算按照当前参数所得的样本不同结果出现的概率，然后计算不同情况的期望，此为E。然后结合实际结果，根据最大似然概率再估计参数值，即为M步。反复迭代直到收敛。步骤如下，设观察到的数据$x=(x_1,x_2,\cdots,x_n)$，联合分布为$p(x,z;\theta)$，条件分布为$p(z|x,\theta)$

• 随机初始化参数$\theta$的初值$\theta_0$
• E: 计算联合分布的条件概率期望； \(Q_i(z_i)=p(z_i|x_i,\theta_i)\\ l(\theta,\theta_j)=\sum_{i=1}^{n}\sum_{z_i}Q_i(z_i)log\frac{p(x_i,z_i;\theta)}{Q_i(z_i)}\)
• M：最大化$l()$，得到$\theta_{j+1}$
• 收敛后输出theta