卡尔曼滤波算法流程图

建立系统模型设定参数根据第K-1时刻的状态预测第K时刻的状态X(K|K-1)根据第K-1时刻的系统预测误差估计第K时刻的系统预测误差P(K|K-1)计算卡尔曼增益Kg计算系统最优估算值X(K|K)计算系统当前时刻的系统预测误差P(K|k)