卡尔曼滤波公式: 公式中变量意义: :k时刻状态估计; :状态转移矩阵;B:控制矩阵;u:控制量; :k+1时刻预测值; :k时刻估计协方差;Q:过程噪声; :k+1时刻预测协方差;H:观测矩阵;R:观测协方差;K:卡尔曼增益;