卡尔曼滤波
状态预测方程是取得是每一项的概率最大值处对应的自变量的值，噪声
对系统状态向量上面加上一个帽子符号表示这是一个估计量，也就是我们实际输出的量。
测量估计方程同样忽略噪声项，变成对此时测量量的估计
递归卡尔曼滤波器所有公式（统一前面卡尔曼滤波的介绍和推导一二中的符号）有
状态预测方程
先验估计误差协方差更新公式
卡尔曼增益计算公式
最优状态估计迭代更新公式
最优(后验)估计误差协方差更新公式
第一行公式没有代入卡尔曼增益的
假设
我们根据测量方程可以反推出状态向量的测量反推值
上述式子中，为了展示
卡尔曼滤波的目标函数为
这里，我们尝试简化一下
这里
这里我们尝试化简一下
在上述公式中， $z_k$ 和 $x_k$ 需要理解为一个常量，而不是服从某一个分布的随机变量。
有上面的公式可以看出，对于上述线性系统的建模后，
同时也可以看出，若后验估计保持了无偏性的传递性，先验估计
一般的材料中，常说卡尔曼滤波的估计值是无偏估计，实际上这是由条件的，其中要求系统噪声和量测噪声为不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，Kalman滤波的估计结果是有偏的。 我们这里是先给出了卡尔曼滤波的一般公式，然后再证明其无偏性。 实际上也可以要求其无偏，设 $x^+_k = K_1 \hat x^-_{k} + K_2 x^{measure}_k$ ，从无偏性中推出 $K_1$ 与 $K_2$ 的关系。
再回到原来的公式
由此可以看出，估计值与真实值之间的均方误差就是估计误差的方差。
我们继续化简
这里用用上了无偏估计的条件，
这里又得出另外一个重要结论，在卡尔曼滤波中，最优估计值是无偏估计的条件下，最小化估计值与真实值之间的均方误差等价于最小化估计值的方差。
因此，卡尔曼滤波的思想即线性融合所有数据来源的分布，求各个分布的系数K使融合后的分布方差最小。
拓展卡尔曼滤波与卡尔曼滤波
普通的卡尔曼滤波仅仅适用于线性系统
而拓展卡尔曼滤波不仅可以用于线性系统,也可以用于非线性系统
Q: 为什么拓展卡尔曼滤波可以使用在非线性系统中?
A: 与求导的思想类似
如果你放大到非线性曲线上的任意一点,你会发现它看起来非常像一条直线.使用扩展卡尔曼滤波器(Extended Kalman Filter),我们通过一个称为雅可比矩阵(Jacobian)的特殊矩阵将非线性方程转换为线性化形式(State Space Model i.e. Transition Function ).然后,我们使用这个线性化形式的方程来完成卡尔曼滤波过程.
State Space Model i.e. Transition Function
reference
Missing \begin{align} or extra \end{align} \begin{bmatrix} x_t \\ y_t \\ \gamma_t \end{bmatrix}= \begin{bmatrix} x_{t-1}+v_{t-1}\cos\gamma_{t-1}*dt \\ y_{t-1}+v_{t-1}\sin\gamma_{t-1}*dt \\ \gamma_{t-1}+\omega_{t-1}*dt \end{bmatrix}= \begin{bmatrix} f_1 \\ f_2 \\ f_3 \end{bmatrix}\\ \begin{bmatrix} x_t \\ y_t \\ \gamma_t \end{bmatrix}=A_{t-1} \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ \gamma_{t-1} \end{bmatrix}+B_{t-1} \begin{bmatrix} v_{t-1} \\ \omega_{t-1} \end{bmatrix} \\ \begin{bmatrix} x_t \\ y_t \\ \gamma_t \end{bmatrix}=\begin{bmatrix} 1,0,0\\0,1,0\\ 0,0,1 \end{bmatrix} \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ \gamma_{t-1} \end{bmatrix}+\begin{bmatrix} \cos \gamma_{t-1}*\mathrm{d}t,0\\sin\gamma_{t-1}*\mathrm{d}t,0\\0,\mathrm{d}t \end{bmatrix} \begin{bmatrix} v_{t-1} \\ \omega_{t-1} \end{bmatrix} \\ \end{align}
Observation Model
reference
Missing \begin{align} or extra \end{align} y_{t}=H\mathbf{x_{t}}+\mathbf{w_{t}}\\ \\ \begin{bmatrix} y_{1}^{t} \\ y_{2}^{t} \\ \cdot\\ \cdot \\ y_{n}^{t} \end{bmatrix}=H\begin{bmatrix} x_{t} \\ y_{t} \\ \gamma_{t} \end{bmatrix}+\begin{bmatrix} w_{1}^{t} \\ w_{2}^{t} \\ \cdot \\ \cdot \\ w_{n}^{t} \end{bmatrix} \end{align}$$ $y$为传感器数值向量,$\mathbf{x_{t}}$为状态向量,$\mathbf{w_{t}}$为噪声向量,$H$为观测矩阵 估计传感器向量\=观测矩阵\*状态向量+噪声向量 $H$同样也是由雅克比行列式推断得到 Q: 传感器数值矩阵为3\*1,状态矩阵为3\*1 什么时候观测矩阵为单位矩阵? A: 观测矩阵形状为3\*3 当传感器矩阵与状态矩阵一一对应的时候,例如传感器1对应状态1 ## State Space Model与Observation Model的联系 State Space Model使用$t-1$时刻的传感器数据,来预测$t$时刻的状态 Observation Model使用$t-1$时刻的状态,来预测$t$时刻的状态,再利用预测状态推断$t$时刻的传感器数据 ## kalman滤波 kalman滤波做的工作是将observation model得到的传感器数值与实际上的传感器数值联系起来,得到一个最优解 Q: 为什么不能够信任实际上的传感器数值? A: 因为传感器并非 100%完美,存在或大或小的噪声 为了减小噪声,使得数据尽量贴近事实,选择使用kalman滤波 
三维俯瞰