原理

Kalman filter algorithm的核心就是五大公式，这五大公式包含了测量值，先验值，后验值。先验值其实就是根据系统模型算出来的近似值(建模存在不准确问题)，而同时我们利用检测设备还可以测得一个测量值，现在问题就是如何将这两个值进行数据融合，计算出一个在该时刻的后验值，这个后验值需满足误差方差最小原则（此时最靠近真实值）。Kalman滤波算法是经过严密推导的数学最优化问题，这里不再推导，给出最终算法的5大公式：

公式中${\hat{X}}_{1,k}^{-}$为先验位置，${\hat{X}}_{1,k}$为后验值(在第k步Kalman滤波后的结果)。在Kalman滤波中需要初始化几个数据，即在0时刻的$\hat{X}$和在1时刻的$P_k$，那么在1时刻可以根据第一个公式计算出先验值，根据第二个公式计算出新的$P_k^{-}$（先验）,之后根据第三个式子计算出$K_k$（这个式子右边是$P_k^{-}$写错了），第四个式子进行先验值和测量值的数据融合，最后一个式子更新$P_k$。

MATLAB实现一个Kalman filter实例

这里我们假想一个物体在一维空间运动，有速度和位置两个状态变量，初始在0位置处，速度为1，系统采样周期为1s，现有卫星观测器观测其位置和速度，但是都存在噪声，我们已经对这个二阶模型建好模了（k时刻位置为k-1时刻位置+速度*1s+噪声，k时刻速度为k-1时刻速度:+噪声），现采用Kalman filter algorithm进行滤波。

代码如下：

%------------------Kalman filter----------------%
%This is an application of Kalman filter algorithm in a simple problem which needs
%us to estimate the displacement and speed. The initial condition is [0;1], which 
%means the x0 = 0, v0 = 1. Besides, the interval time is 1s. p(w) and p(v) are the noise
%which obey normal distribution and the covariance matrices are Q and R respectively.

H = eye(2); A = [1 1;0 1];
Pk = eye(2); Q = 0.1*eye(2);R = eye(2);
X_k = [0;1]; X_hat = [0;1];

X_kdata = [0;1]; X_hatdata = [0;1];
X_hat_data = []; Z_kdata = [];
iter = 20;
for i = 1:iter
    pw = mvnrnd([0 0],[0.1 0;0 0.1],1)';
    pv = mvnrnd([0 0],[1 0;0 1],1)';
    X_k = A*X_k + pw; X_kdata = [X_kdata,X_k];
    Z_k = H*X_k + pv; Z_kdata = [Z_kdata,Z_k];
    
    Pk_ = A*Pk*A'+Q;
    Kk = Pk_*H'*inv(H*Pk_*H'+R);
    X_hat_ = A*X_hat; 
    X_hat_data = [X_hat_data,X_hat_];
    X_hat = X_hat_ + Kk*(Z_k - H*X_hat_);
    X_hatdata = [X_hatdata,X_hat];
    Pk = (eye(2) - Kk*H)*Pk_;
end

figure(1);
plot(0:iter,X_kdata(1,:),0:iter,X_hatdata(1,:)...
    ,1:iter,X_hat_data(1,:),1:iter,Z_kdata(1,:)...
    ,0:iter,X_kdata(2,:),0:iter,X_hatdata(2,:)...
    ,1:iter,X_hat_data(2,:),1:iter,Z_kdata(2,:));
legend("实际位置","后验位置","先验位置","测量位置",...
    "实际速度","后验速度","先验速度","测量速度"...
    ,"Location","best");
grid on; title("Kalman filter");
xlabel("iter"); ylabel("x or v");

• 结果
  
• 局部放大
  

可以看到Kalman filter实际上是一个数据融合的过程，测的相较于算的不准的时候就偏向于算的，算的相较于测得不准的时候就偏向于测的，而这个融合的系数$K_k$是最优化误差方差得到的。