卡尔曼滤波

卡尔曼滤波是一个纯时域的滤波器，不需要进行频域变换，描述状态之间的线性关系。
卡尔曼滤波主要由两部分组成：
1、预测：使用上一阶段的状态来预测当前状态的预测值。
2、校正：利用对当前状态的观测值修正在预测阶段获得的预测值，获得一个更接进真实值的新估计值。

公式推导

预测

状态预测公式：
$$\widehat{x_t^{-}}=F_t\widehat{x_{t-1}}+B_t{u}_t$$
其中$\widehat{x_t^{-}}$表示当前状态的估计值,’-'上标表示现在的估计值是根据上一状态预测的，还需用观测值进行校正，才可得到最优的当前状态估计值$\widehat{x_t}$，$F_t$表示当前状态转移矩阵，用于从上一时刻的状态推测当前时刻的状态，$\widehat{x_{t-1}}$表示上一状态的估计值，$B_t$是控制矩阵，表示控制量$u_t$如何作用于当前状态。

噪声是不确定的，假设噪声成高斯分布，利用协方差矩阵来表示噪声不同维度的相关程度。(以两个维度为例)
$$cov(x)=\left[\begin{array}{cc}{\sigma }_{11}& {\sigma }_{12}\\ {\sigma }_{12}& {\sigma }_{22}\end{array}\right]$$
其中${\sigma }_{11}$为第一个维度的方差，${\sigma }_{22}$为第二个维度的方差，${\sigma }_{12}$为两个维度的协方差。如果两个维度不相关，${\sigma }_{12}=0$，正相关，${\sigma }_{12}>0$，负相关，${\sigma }_{12}<0$。
所有不确定性的因素，都用协方差矩阵表示。

不确定性转移公式：
$$P_t^{-}=F_t{P}_{t-1}{F}^T+Q$$
其中$P$表示协方差矩阵。协方差矩阵的性质：$cov(Ax,Bx)=Acov(x,x)B^T$。噪声的预测也并非完全准确，因此加上协方差矩阵$Q$，表示预测噪声本身带来的噪声。

校正

观察值公式：
$$Z_t=H{x}_t+v$$
其中，$Z_t$表示观测值，$H$表示由状态到观测值的变换关系矩阵，即观测矩阵，$v$表示观测的噪声。观测噪声的协方差矩阵为$R$。

状态更新：
$$\widehat{x_t}=\widehat{x_t^{-}}+K_t(Z_t-H\widehat{x_t^{-}})$$
$Z_t-H\widehat{x_t^{-}}$表示实际观测值与预期观测值之间的残差。将残差与$K_t$相乘即可用于修正$\widehat{x_t}$。
实际观测值与预期观测值的残差的协方差为：$$S_t=H{P}_T^{-}{H}^T+R$$

$K_t$为卡尔曼系数，其公式为：$$K_t=P_t^{-}{H}^T{S}_t^{-1}=P_t^{-}{H}^T(H{P}_T^{-}{H}^T+R{)}^{-1}$$
其作用为：
1、利用预测模型的协方差矩阵和观测模型的协方差矩阵，用于决定相信预测模型多一些还是观察模型多一些。相信预测模型多一些，残差的权重就会小一些，否则就会大一些。
2、将残差的表现形式从观察域转换到状态域。

更新噪声最佳估计值的分布：
$$P_t=(I-K_{t}H)P_t^{-}$$
卡尔曼滤波在不确定性的变化中寻找平衡，不断迭代，不确定性噪声不断发生改变。

代码例子

%% 以匀速小汽车为例
z=6:2:204;%理想观测值
speed=2*ones(1,100);
noise_z=randn(1,100);%方差为1的高斯噪声
z=z+noise_z;%添加了噪声的观测值
x=[0;0];%初始状态
P=[1 0;0 1];%状态协方差矩阵
F=[1 1;0 1];%状态转移矩阵
Q=[0 0;0 0];%状态转移协方差矩阵
H=[1 0];%观测矩阵
R=1;%观测噪声方差
Kalman_ans=[];
for i=1:100 %根据卡尔曼滤波的公式进行迭代
    x_t=F*x;
    P_t=F*P*F'+Q;
    K_t=P_t*H'*pinv(H*P_t*H'+R);
    x=x_t+K_t*(z(i)-H*x_t);
    P=(eye(2)-K_t*H)*P_t;
    Kalman_ans=[Kalman_ans;x'];
end
plot(Kalman_ans(:,1),Kalman_ans(:,2));
hold on;
plot(z,speed);
legend({'卡尔曼滤波预测值','观测值'});
xlabel('位置'); ylabel('速度');

结果图：