一维卡尔曼滤波

公式

I = 1;
if xk_1 == 0 
    xk_1 = x;
    xk = x;
else
    zk = H*x;                   % 观测量方程
    % 预测
    X = A*xk_1;                 % 状态预测             
    P = A*Pk_1*A' + Q;          % 误差协方差预测 	P = Pk-1 +Q
    % 更新(校正)
    K = P*H'*inv(H*P*H'+R);     % 卡尔曼增益更新	K = P * inv(P + R)
    xk = X + K*(zk - H*X);      % 更新校正	
    xk_1 = xk;                  % 保存校正后的值，下一次滤波使用	
    Pk = (I - K*H)*P;           % 更新误差协方差	Pk = (1 - K *H)*p
    Pk_1 = Pk;                  % 保存校正后的误差协方差，下一次滤波使用
end

实验

以下是我的卡尔曼滤波代码

def kalman_filter(self,input_array,Q=0.0001,R=0.005):
        # r 测量噪声， Q预测噪声
        out_array = []
        xk = 0
        pk2_k = R*R
        for i in range(len(input_array)):
            if(i >= 1):
                # zk = input_array[i]	k = xk + 0.005*(input_array[i]-xk)
                zk = input_array[i]
                pk2_k_1 = pk2_k + Q*Q	
                kk = math.sqrt(pk2_k_1/(pk2_k_1 + R*R))
                xk = xk + kk*(zk - xk)
                pk2_k = (1-kk)*pk2_k_1
                out_array.append(xk)n 
            else:
                out_array.append(input_array[0])
        return out_array

疑问

经过实验对比，用注释的一句代码替换代码的9-14行并没什么区别。注释的那句话用的是互补滤波的方法写的。

卡尔曼滤波和互补滤波相比，主要特点是增益Kg可以变动的，会根据方差等计算出一个最优的增益。
不过这个公式里面的Kg跟输入一点关系都没有，只和Q和R有关，算来算去，就是这个增益按照一定的规则变化而已。
那为什么会有低通滤波的效果呢，因为你的Q值很小，这就导致了这个增益很小，更加信任上一次的值。

其实说到底这段程序就是上一次的值加上这次测量值的乘以一个很小的系数，而上一次的值又是之前的值累加起来的。
如果这个系数（增益）保持固定，就是普通的一阶低通滤波。

一维的卡尔曼滤波退化成了一个普通的低通滤波。

而这个增益的话，我觉得可以用自己的逻辑去控制，不需要用卡尔曼滤波这种看似复杂的算法。