卡尔曼滤波器是最佳线性滤波器，有着实现简单并且纯时域变换的特点。

只写了一半

卡尔曼滤波器公式

$$
\begin{align}
\hat{x}t^- & = F\hat{x}{t-1} + Bu_t \
pt^- & = FP{t-1}F^T+Q \
\hat{x}_t & = \hat{x}_t^- + K_t(Z_t-H\hat{x}_t^-) \
K_t & = P_t^-H^T(HP_t^-H^T+R)^{-1} \
P_t & = (I-K_tH)P_t^-
\end{align}
$$

简单小车运动模型

运动学公式

假设小车在路上行驶，其状态用
$$
\begin{align}
\hat{x}_{t-1}^- = [\begin{matrix} p \ v \end{matrix}]
\end{align}
$$
p是小车在t-1时刻的位置，v是小车在t-1时刻的速度。


在不考虑状态量$u_t$的情况下，下一时刻状态为
$$
\begin{align}
\hat{x}t^-=F\cdot \hat{x}{t-1}^-
\end{align}
$$

其中F为状态转移矩阵，在该模型中，F为

$$
\begin{align}
F = [\begin{matrix} 1 & \Delta t \ 0 & 1 \end{matrix}]
\end{align}
$$

在考虑状态量$u_t$的情况下，得到卡尔曼滤波器的第一个公式

$$
\begin{align}
\hat{x}t^- & = F\hat{x}{t-1} + Bu_t
\end{align}
$$

在该模型中，我们假设这个控制量为加速度，则矩阵B可以表示为

$$
\begin{align}
B = [\begin{matrix} \cfrac{1}{2}\Delta t^2 \ \Delta t \end{matrix}]
\end{align}
$$

这样我们得到运动学公式

噪声

在这里，我们假设的噪声都是高斯分布的噪声，其方差为$\sigma $。

在该模型中协方差矩阵为

$$
\begin{align}
P{t-1} =[
\begin{matrix}
\sigma {11} &\sigma {12}\
\sigma {21} &\sigma _{22}
\end{matrix}]
\end{align}
$$

这里正对角系数 $\sigma {11}$ 和 $\sigma {22}$ 分别为位置和速度的测量噪声的方差。

反对角系数 $\sigma {12} = \sigma {21}$ 为噪声的相关性

$$
\begin{aligned}
\dot{x} & = \sigma(y-x) \
\dot{y} & = \rho x - y - xz \
\dot{z} & = -\beta z + xy \
\dot{z} & = x_x
\end{aligned}
$$