卡尔曼滤波器由以下五个方程构成，其中前两个是时间更新方程，后面三个是状态更新方程。时间更新方程是根据已知模型用上一时刻状态估计当前时刻的状态。状态更新方程是在得到当前时刻系统输出测量值后对当前时刻状态进行更新修正。
\begin{cases}\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1}\\P_k^-=AP_{k-1}A^T+Q\\K_k=P_k^-H^T(HP_k^-H^T+R)^{-1}\\\hat{x}_k=\hat{x}_k^-+K_k(Z_k-H\hat{x}_k^-)\\P_k=(I-K_kH)P_k^-\end{cases}
其中–表示先验，^表示估计，k表示k时刻的值。u是系统输入，x是系统状态,z是系统输出。A是系数矩阵，B是控制矩阵，H是输出矩阵。P是误差协方差矩阵，Q是过程激励噪声协方差矩阵，R是观测噪声协方差矩阵，K是卡尔曼系数。
1 工作原理
在介绍卡尔曼滤波器前先介绍常见滤波器的缺点。常用的低通滤波器、带阻滤波器等都是从频域考虑的。先计算或测试控制对象能响应的频率，然后滤除不想要的频率。以两轮车的惯性导航单元用的滤波器为例。
2 公式证明
2.1系统模型
使用状态空间的概念。考虑到实际测量误差，在每次更新时认为叠加了一个噪声信号。滤波器的任务就是将这个噪声信号的影响降到最低。
使用状态空间模型表示一个线性系统：
\begin{cases}x_k=Ax_{k-1}+Bu_{k-1}+\omega_{k-1}\\z_k=Hx_k+\vartheta_k\end{cases}
其中$\omega$和$\vartheta$分别表示过程激励噪声和观测噪声。这里设他们为相互独立，正态分布的白噪声，即：
\begin{cases}p(\omega)\sim N(0,Q)\\p(\vartheta)\sim N(0,R)\end{cases}
定义先验估计误差和后验估计误差
e_k^-\equiv x_k-\hat{x}_k^-\\e_k\equiv x_k-\hat{x}_k
他们的协方差分别为
P_k^-=E[e_k^-(e_k^-)^T]\\P_k=E[e_ke_k^T]
2.2 根据模型更新误差协方差
将状态空间模型带入先验误差的的定义得
\begin{aligned}e_k^-&=x_k-\hat{x}_k^-\\&=Ax_{k-1}+Bu_{k-1}-A\hat{x}_{k-1}-Bu_{k-1}-\omega_{k-1}\\&=Ae_k-\omega_{k-1}\end{aligned}
它的协方差为：
\begin{aligned}Pk^-&=E[e_k^-(e_k^-)^T]\\&=AE[e_{k-1}e_{k-1}^T]A^T+E[\omega_{k-1}\omega_{k-1}^T]\\&=AP_{k-1}A^T+Q\end{aligned}
2.3 卡尔曼系数更新
将状态更新方程带入后验估计误差定义式得
\begin{aligned}e_k&\equiv x_k-\hat{x}_k\\&=x_k-\hat{x}_k^--K(z_k-H\hat{x}_k^-)\\&=e_k^--K(Hx_k+\vartheta_k-H\hat{x}_k^-)\\&=e_k^--K(He_k^-+\vartheta_k)\end{aligned}
其中$z_k-H\hat{x}_k^-$是测量过程的残余，为测量变量与预测值之差
令 tr[\frac{\mathrm{d}P_k}{\mathrm{d}K}=]=0 ，得
K=HP_k^-\cdot(HP_K^-H^T+R)^{-1}
2.4 根据测量数据更新误差协方差
\begin{aligned}e_k&=x_k-\hat{x}_k\\&=x_k-\hat{x}_k^- -K_k(z_k-H\hat{x}_k^-)\\&=e_k^- -K_k(He_k^-+\vartheta_k)\\&=(I-K_kH)e_k^-+K_k\vartheta_k\\P_k&=E[e_k\cdot e_k^T]\\&=(I-K_kH)P_k^-(I-K_kH)^T+K_kRK_k^T\\&=P_K^- -K_kHP_k^- - P_k^-H^TK_k^T+K_k(HP_k^-H^T+R)K_k^T\end{aligned}代入 K_k=HP_k^-(HP_kH^T+R)^{-1} 得
\begin{aligned}P_k&=P_k^- -HP_k^-(HP_kH^T+R)^{-1}HP_k^-\\&=P_k^- -K_kHP_k^-\\&=(I-K_kH)P_k^-\end{aligned}