卡尔曼滤波是一个纯时域的滤波器，不需要进行频域变换，描述状态之间的线性关系。
卡尔曼滤波主要由两部分组成：
1、预测：使用上一阶段的状态来预测当前状态的预测值。
2、校正：利用对当前状态的观测值修正在预测阶段获得的预测值，获得一个更接进真实值的新估计值。
状态预测公式：
x
t
−
^
=
F
t
x
t
−
1
^
+
B
t
u
t
\hat{x_t^-}=F_t\hat{x_{t-1}}+B_tu_t
xt−^=Ftxt−1^+Btut
其中
x
t
−
^
\hat{x_t^-}
xt−^表示当前状态的估计值,’-'上标表示现在的估计值是根据上一状态预测的，还需用观测值进行校正，才可得到最优的当前状态估计值
x
t
^
\hat{x_t}
xt^，
F
t
F_t
Ft表示当前状态转移矩阵，用于从上一时刻的状态推测当前时刻的状态，
x
t
−
1
^
\hat{x_{t-1}}
xt−1^表示上一状态的估计值，
B
t
B_t
Bt是控制矩阵，表示控制量
u
t
u_t
ut如何作用于当前状态。
噪声是不确定的，假设噪声成高斯分布，利用协方差矩阵来表示噪声不同维度的相关程度。(以两个维度为例)
c
o
v
(
x
)
=
[
σ
11
σ
12
σ
12
σ
22
]
cov(x)=\begin{bmatrix} {\sigma_{11}}&{\sigma_{12}}\\ {\sigma_{12}}&{\sigma_{22}} \end{bmatrix}
cov(x)=[σ11σ12σ12σ22]
其中
σ
11
\sigma_{11}
σ11为第一个维度的方差，
σ
22
\sigma_{22}
σ22为第二个维度的方差，
σ
12
\sigma_{12}
σ12为两个维度的协方差。如果两个维度不相关，
σ
12
=
0
\sigma_{12}=0
σ12=0，正相关，
σ
12
>
0
\sigma_{12}>0
σ12>0，负相关，
σ
12
<
0
\sigma_{12}<0
σ12<0。
所有不确定性的因素，都用协方差矩阵表示。
不确定性转移公式：
P
t
−
=
F
t
P
t
−
1
F
T
+
Q
{P_t^-}=F_t{P_{t-1}}F^T+Q
Pt−=FtPt−1FT+Q
其中
P
P
P表示协方差矩阵。协方差矩阵的性质：
c
o
v
(
A
x
,
B
x
)
=
A
c
o
v
(
x
,
x
)
B
T
cov(Ax,Bx)=Acov(x,x)B^T
cov(Ax,Bx)=Acov(x,x)BT。噪声的预测也并非完全准确，因此加上协方差矩阵
Q
Q
Q，表示预测噪声本身带来的噪声。
观察值公式：
Z
t
=
H
x
t
+
v
Z_t=Hx_t+v
Zt=Hxt+v
其中，
Z
t
Z_t
Zt表示观测值，
H
H
H表示由状态到观测值的变换关系矩阵，即观测矩阵，
v
v
v表示观测的噪声。观测噪声的协方差矩阵为
R
R
R。
状态更新：
x
t
^
=
x
t
−
^
+
K
t
(
Z
t
−
H
x
t
−
^
)
\hat{x_t}=\hat{x_t^-}+K_t(Z_t-H\hat{x_t^-})
xt^=xt−^+Kt(Zt−Hxt−^)
Z
t
−
H
x
t
−
^
Z_t-H\hat{x_t^-}
Zt−Hxt−^表示实际观测值与预期观测值之间的残差。将残差与
K
t
K_t
Kt相乘即可用于修正
x
t
^
\hat{x_t}
xt^。
实际观测值与预期观测值的残差的协方差为：
S
t
=
H
P
T
−
H
T
+
R
S_t=HP_T^-H^T+R
St=HPT−HT+R
K
t
K_t
Kt为卡尔曼系数，其公式为：
K
t
=
P
t
−
H
T
S
t
−
1
=
P
t
−
H
T
(
H
P
T
−
H
T
+
R
)
−
1
K_t=P_t^-H^TS_t^{-1}=P_t^-H^T(HP_T^-H^T+R)^{-1}
Kt=Pt−HTSt−1=Pt−HT(HPT−HT+R)−1
其作用为：
1、利用预测模型的协方差矩阵和观测模型的协方差矩阵，用于决定相信预测模型多一些还是观察模型多一些。相信预测模型多一些，残差的权重就会小一些，否则就会大一些。
2、将残差的表现形式从观察域转换到状态域。
更新噪声最佳估计值的分布：
P
t
=
(
I
−
K
t
H
)
P
t
−
P_t=(I-K_tH)P_t^-
Pt=(I−KtH)Pt−
卡尔曼滤波在不确定性的变化中寻找平衡，不断迭代，不确定性噪声不断发生改变。
%% 以匀速小汽车为例
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('速度');
结果图：
