Kalman 计算学习小结

算法推导此处不做赘述；

实现：

    P[i] =  (P[i-1]) +Q          #(Q)  #P[i]协方差；

 
    K[i] =  np.sqrt( P[i]/(P[i] +  R  ))     #(R)    #(np.sqrt取开方);单纯增益

 
    Y[i] =  Y[i-1] + K[i] * (S[i] - Y[i-1]) #S[i]当前传感器测量数据；Y[i-1]上一次预估输出数据

     
    P[i] = ((1-K[i])*P[i])               

实现：

P_:=P_old +Q_in;  (*P_old需静态变量*)

kalman_K := SQRT(P_/(P_ + R_in));

Y_out := Y_old + kalman_K * (Measure_in - Y_old );(*Y_old需静态变量*)

Y_old := Y_out;


P_old := (1 - kalman_K) * P_ ;

一般状态下用真值调试当前模型的Q/R。（一般真值测量设备很昂贵因此需要用卡尔曼调试）。