python起步之卡尔曼滤波

关于卡尔曼滤波的理论这里不打算讲了，就是那个5个基本的公式， 这里直接给出公式

公式1： X(k|k-1) = AX(k-1 | k-1) + BU(k) + W(k)

公式2：P(k|k-1)  = AP(k-1|k-1)A' + Q(k)

公式3：   X(k|k)  = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)

公式4：    Kg(k)  = P(k|k-1)H'/{HP(k|k-1)H' + R}        //卡尔曼增益

公式5 ： P(k|k)   = (1- Kg(k) H) P(k|k-1)

另外，Z(k) = HX(k) + V，Z是测量值，X是系统值，W是过程噪声，V是测量噪声，H是测量矩阵，A是转移矩阵，Q是W的协方差，R是V的协方差，X(k|k-1)是估计值；X(k|k)是X