Kalman滤波: 其中 Ak为状态转移矩阵, R为状态估计噪声, Q为观测噪声, Ck为观测矩阵,, Pk为状态的协方差,需要对 P0初始化。 Extended Kalman滤波: 预测: 更新卡尔曼增益: