随着传感技术，机器人，自动驾驶等不断发展，对控制系统的精度以及稳定性要求越来越高。卡尔曼滤波作为一种状态最优估计方法，应用也越来越普遍、
对于Kalman Filter的理解，用过都知道“黄金五条”公式，且通过“预测”和“更新”两个过程对系统进行最优估计。
简单来说，卡尔曼滤波就是将观测值（指从系统中获得的实际测量值，如：传感器测量值，GPS定位值等）和控制量（系统中的外部输入，如：控制器输出的控制指令，系统的的运动状态等）
进行融合。得到对系统状态（描述某一时刻的状态量或状态向量，它由系统内部的各种物理量，状态变量组成。如：对于飞行器来说，系统状态可以包括飞行器的位置，速度，姿态，加速度等信息）的估计值。来进行预测。
卡尔曼滤波：用来估计系统状态的算法，它通过测量结果和预测结果进行加权平均，来得到更准确的状态估计值。
包括两个方面：状态预测和状态更新。
状态预测：第一步预测系统的状态。这个预测是基于系统的上一个状态和控制输入（如果有的话) 计算出来的。预测结果包含了系统的状态和状态估计的不确定性（协方差矩阵）。
状态更新：第二步是将测量结果与预测结果进行加权平均，来得到更准确的状态估计值。加权平均的权重是由预测结果的不确定性和测量结果的不确定性共同决定的。如果预测结果的不确定性很小，那么
预测结果占主导地位；如果测量结果的不确定性很小，那么测量结果会占主导地位。
提出背景：
火箭在发生过程中需要时刻预测自己的状态（位置，速度），我们可以用一些传感器获得这两个量，但由于存在电离层和大气层的干扰，传感器的测量值有时误差会很大，我们需要一种方法，预测一个新的更加准确的，且不完全依赖传感器返回值的状态信息。
事实上，如果传感器的返回值非常离谱，那我们完全有理由不相信此时的数据，因为火箭需要符合某个运动方程，即使存在一些干扰，火箭的状态也应该是在某个合理的范围内。
思路：把传感器观测值和理论预测值做加权平均。既不完全相信传感器，也不完全相信理论模型。
假设我们只需要观测一个值，即设计一个一维Kalman Filter。
由于理论模型和观测都存在相应干扰，假设二值均服从正态分布（生活中几乎到处都是正态分布）
理论模型值的概率分布和观测值的概率分布只能通过大量数据采样得到，这要求我们在应用上述方法进行滤波前，应该通过大量观察对理论模型和观测模型的分布有一个准确的了解。实际工程我们一般假设传感器具有某个确定分布的，这个分布可以对传感器进行一段时间的观测得到（如 slam中经常会让系统静止两小时以计算传感器的分布）。而系统方差是会随着我们每一次进行卡尔曼融合而改变的（因为状态矩阵a的影响），所以我们需要持续的计算系统方差-- - 迭代。
我们能做的就是系统运行开始后计算每一次的理论值方差，根据这个方差和传感器方差计算出融合权重，每次都要重新计算-- 迭代。
每进行一次观测，就需要对两个分布的方差重新计算（一般传感器的方差不重新计算，设为定值），迭代下去。
因为是迭代的，考虑第k个时刻，当前的最优估计值应该由根据之前时刻状态由运动模型计算出来的理论值，和tk时刻的传感器测量值，融合计算出tk时刻的卡尔曼预测值。
注意：状态更新每次都需要上一时刻的真实状态作为输入，而我们无法获得真实的输入，只能拿上一时刻有卡尔曼理论预测出来的作为输入计算理论值。
状态转移矩阵（系统模型矩阵）：表示系统在一个离散时间步长内从一个状态转移到下一个状态时的线性关系，是一个方阵。描述了系统的动态特性，它将当前时刻的状态向量与下一个时刻的状态向量之间的关系进行描述。在卡尔曼滤波中，状态转移矩阵F是通过系统的物理模型或者观测到的数据来估计或确定的。它可以包括系统的动态方程，控制输入，噪声等因素。
控制输入矩阵：是在系统状态转移过程中考虑外部控制输入对系统状态变化的影响而引入放入一个矩阵，表示了系统状态变化对外部控制输入的响应关系。作用：增强卡尔曼滤波算法对外部控制的建模能力，提高状态估计的准确性。通过调整控制输入矩阵，可以对系统的控制策略进行优化，以达到更好的状态估计效果。
状态协方差矩阵：是卡尔曼滤波算法中用于描述对系统状态估计的不确定的矩阵。它表示状态向量各个状态变量之间以及同一个状态变量在不同时刻之间的协方差关系。
例：假设一个简单系统，其状态向量包含两个状态变量：位置和速度，用x表示位置，v表示速度，则状态向量为X= [ x; v] ;
状态协方差矩阵是一个与状态向量维度一致的对称矩阵，它表示了位置和速度之间以及同一个状态变量在不同时刻之间的协方差关系，假设状态协方差矩阵为P，则P的元素可以表示为：
P = [ Pxx Pxv;
Pvx Pvv] 其中，Pxx表示位置x的方差，Pvv表示速度v的方差，Pxv和Pvx表示位置x和速度v之间的协方差。
例：我们考虑一个移动的物体，通过传感器测量物体的位置和速度。初始时刻，我们对物体的位置和速度没有任何信息，因此可以将状态协方差矩阵P初始化为一个较大的值，表示对状态估计不确定较高。
随着时间的推移，我们通过状态转移方程和测量方程，不断更新状态估计和状态协方差矩阵。例：在一个时间步骤中，根据物体的运动模型和先前的状态估计，可以预测物体下一时刻的位置和速度，并更新状态协方差矩阵。
在更新状态协方差矩阵时，如果物体在该时间被传感器测量到了位置和速度的值，我们可以使用测量结果来修正状态协方差矩阵。通过卡尔曼增益的计算。。。。
一 . 入门
1. 引入
2. 适用系统：线性高斯系统
线性：y= kx+ b z= ax+ by w= u^ 2
满足（叠加性和其次性）
1. 叠加性 y= ax1+ bx2;
2. 其次性 kx-- > ky
高斯：噪声满足正态分布
3. 宏观意义：滤波即加权
理想状态：信号* 1 + 噪声* 0 ；
低频 1 高频0 低通滤波
估计（） 观察( ) 卡尔曼滤波
修正
