1、卡尔曼滤波公式如下
（1）预测方程：
预测状态向量=转换矩阵*上一时刻更新的状态向量 + 控制矩阵*当前的系统输入
/----------------------P推导 begin-----------------------------/
预测系统状态的协方差矩阵 = E[(状态向量-预测状态向量）(状态向量-预测状态向量)转置]
预测系统状态协方差矩阵 = 转换矩阵*上一时刻系统状态协方差矩阵*转换矩阵的转置+随机噪声*随机噪声转置
/----------------------P推导 end-----------------------------/
（2）更新方程：
/---------------------------------------辅助公式 begin-------------------------------------------------/
当前时刻测量值 = 测量矩阵*当前状态向量 + 测量噪声
测量值估计 = 测量矩阵*预测状态向量
/--------------------------------------辅助公式 end--------------------------------------------------/
状态向量估计 = 预测状态向量 + 卡尔曼增益(当前测量值-测量估计值)
卡尔曼增益 = 系统协方差矩阵*测量矩阵转置/（测量矩阵*系统协方差矩阵*测量矩阵转置）+高斯噪声
系统协方差矩阵 = 预测系统协方差矩阵 - 卡尔曼增益*测量矩阵*预测系统协方差矩阵
/-----------------P+推导 begin------------------------/
状态向量1：
状态向量2：
它们符合高斯分布，如下有，Rt为高斯噪声，
这两个状态向量的来源我们可以认为是独立的，因此他们的联合概率分布是各自概率分布的乘积。***重点是，高斯分布的乘积依旧是高斯分布！！！！***新的高斯分布的均值和方差有如下表达形式：
/-----------------P+推导 end----------------------/