一、卡尔曼滤波简介KF

卡尔曼滤波器（Kalman Filter）是一种用于估计动态系统状态的递归算法。它通过结合系统的动态模型和噪声观测数据，提供对系统状态的最优估计。卡尔曼滤波器广泛应用于信号处理、控制系统、导航、计算机视觉等领域。
卡尔曼滤波器基于线性系统模型，假设系统的动态过程和观测过程都受到高斯噪声的影响。滤波器在两个主要步骤之间交替进行：
预测（Prediction）：根据系统的动态模型，利用前一时刻的状态估计来预测当前时刻的状态和协方差矩阵。
更新（Update）：结合当前的观测数据，对预测的状态和协方差进行校正，得到改进的状态估计。

• 应用

1. 导航和定位：在GPS和惯性导航系统中，用于实时估计位置、速度和姿态。
2. 信号处理：用于滤除信号中的噪声，提取有用信息。
3. 控制系统：在自动控制系统中，用于估计系统状态，提高控制精度。
4. 计算机视觉：在目标跟踪和物体识别中，用于估计物体的位置和速度。
5. 经济学和金融学：用于时间序列分析和预测经济指标。
   1.1 卡尔曼滤波核心思想
   滤波（Filtering）是一种用于处理信号或数据的技术，其目的是从原始数据中提取出所需的信息并去除噪声、误差或不相关的信息。卡尔曼滤波结合了系统的动态模型和传感器测量数据，通过递归地更新估计状态来提供对系统状态的最优估计。它通过最小化状态估计的均方误差来优化估计结果，因此被称为最优滤波器。
   卡尔曼滤波的核心思想是:假定观测的系统是线性的,噪声都满足高斯分布。这一刻系统的状态(最优估计)是这一刻的预测值和这一刻的测量值的加权平均,当得到最优估计之后,再将这一刻的最优估计和估计值进行对比,如果相差比较小,则说明估计比较准确,下次计算就加大估计值的权值,否则说明估计值不准确,下次计算就加大测量值的权值。重复以上过程。

                              当前值=K预测值+（1-K）观测值
   

   卡尔曼滤波算法的本质，就是基于数值的不确定性调节这个参数K（卡尔曼增益）。在迭代若干次后逼近输出值不确定性最小的数值。
   

   1.2 卡尔曼滤波公式推导

   宏观理解

   卡尔曼滤波分为预测和更新两步
   1.使用上一时刻的最优结果预测当前的值
   2.使用当前时刻的观测值修正
   5个核心公式的关系如下图所示
   

• 公式1：基于过去时刻的状态以及控制量对当前时刻的状态进行估计，注意，数据是先验的。
• 公式2：计算公式1的先验估计状态的协方差矩阵，前一部分是由上次最优估计值自身引入的协方差，后一部分是本次估计产生的不确定性（比如无法预测的外部扰动以及模型自身的不确定性）。
• 公式3：计算卡尔曼增益（参数K），这个参数决定了相信估计值还是相信测量值多一些。
• 公式4：就是根据公式4的卡尔曼增益计算得到的最优状态估计值
• 公式5：计算本次最优状态估计值的协方差矩阵，这个数据在下一次迭代的公式2中被用到。

  1.3 公式推导

  下面通过一个例子，明白卡尔曼滤波公式的原理，假设一个匀加速行驶的小车，由卫星GPS的值，估计出小车的速度和位置
  
• Step1 先验估计
  通过运动学公式根据前一时刻的速度ν_(i-1)和常量加速度a可以得到如下公式
  
  
  可简化表示为：
  
  
• Step2 先验估计协方差
  协方差表示两个随机变量之间的关系强度及方向，即它们的联合变化程度。状态协方差矩阵（State Covariance Matrix）表示对系统状态估计的不确定性程度。状态协方差矩阵描述了系统状态向量中每个状态量之间的关系，以及它们的不确定性大小。随着滤波器的更新，状态协方差矩阵会根据系统动态模型和传感器测量进行调整，以反映对系统状态的最新估计和置信度。
  
  
  可简化表示为：
  
  
• Step3 测量方程
  GPS只能获取位置信息，则预测方式表达如下
  
  
  
  H 矩阵的作用是将系统状态的估计值映射到传感器的测量空间中，以便将状态估计与传感器测量进行比较和融合。H 矩阵通常是一个稀疏矩阵，其中非零元素表示系统状态向量中的哪些部分与传感器测量直接相关。
• Step4 修正估计
  融合预测位置和观测位置，卡尔曼增益为K，x ̂_t^-为预测值，z_t是观测值，更信任预测值K_t小，更信任观测值K_t大，最优估计值x ̂_t可被表示为
  
  
  一维情况下H矩阵为1，R为观测噪声的方差，Q为预测值噪声的方差，则卡尔曼增益K_t可被表示为：
  
  
• Step5 更新后验估计的协方差
  更新最优估计值x ̂_t的协方差可被表示为，作为下一时刻的协方差矩阵
  
  
  
  其中过程中的Q和R是人为设定的作为超参数，也是卡尔曼滤波的一个优化点
• 使用卡尔曼滤波的一般步骤

1. 选择状态量、观测量
2. 构建方程
3. 初始化参数
4. 代入公式迭代
5. 调节超参数
6. 关于QR
   在实际的工程应用中，这些数值是需要实测，不过我们可以从公式发现，只要固定估计噪声和测量噪声的方差的比值Q/R计算的结果就是一样的
   H矩阵
   在推导公式的时候我们就觉得这个H非常碍眼（我敲公式的时候也是），但是在实际写代码的时候，我们可以认为的把H设为单位矩阵。只要确保传感器的数值是反应直接测量的状态
   初始值
   迭代的初始需要提供估计值的初值。为了提高数据的收敛速度，最好把协方差矩阵 Pt 初始值设为非零数，即开始的状态基于测量值（当然，如果你有非常可靠的状态初值，建议把协方差矩阵 Pt 初始值设为零）。

   二、扩展卡尔曼滤波EKF

   扩展卡尔曼滤波可以通过泰勒展开来解决非线性情况下的问题。扩展卡尔曼滤波是标准卡尔曼滤波在非线性情形下的一种扩展形式，EKF算法是将非线性函数进行泰勒展开，省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化，最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。
   对于预测和观测方程有
   
   
   是展开后得到的一阶偏导项。在展开过程中省略了次数较高的项，将状态估计方程转化为 近似的线性化方程。其余步骤和卡尔曼滤波一致。对于线性卡尔曼滤波器，H是在运行滤波器之前计算的恒量。对于EKF，H在每一步都要进行更新，因为状态量x点在每个循环后都会发生变化。
   对于预测和观测方程有