08 状态估计

08 状态估计

• 目录
• 0 磁罗盘的标定
• 1 状态估计简介
• 1.1 状态估计
• 2 卡尔曼滤波（KF）
• 2.1 卡尔曼滤波简介
• 2.2 隐马尔科夫模型
• 2.2.1 系统模型
• 2.2.2 线性系统模型
• 2.2.3 卡尔曼滤波器
• 2.2.4 新息卡方检验法
• 3 扩展卡尔曼滤波（EKF）
• 3.1 扩展卡尔曼滤波器
• 3.2 线性化
• 3.3 两轮车模型仿真
• 4 互补滤波
• 4.1 线性互补滤波
• 4.1.1 基本思想
• 4.1.2 以俯仰角为例
• 5 多采样频率与多延时下姿态估计策略
• 5.1 实际工程与理论的差别
• 5.2 姿态传感器采样和延迟效应的建模
• 5.3 问题描述
• 5.4 问题解决
• 6 PX4中的状态估计

目录

0. 磁罗盘的标定
1. 状态估计简介
2. 卡尔曼滤波（KF）
3. 扩展卡尔曼滤波（EKF）
4. 互补滤波
5. 多采样频率与多延时下姿态估计策略
6. PX4中的状态估计

---

0 磁罗盘的标定

• 由于地磁矢量与磁罗盘姿态无关，因此理论上当旋转磁罗盘时，测量量将形成一个球面。

  • 旋转磁罗盘时，可以认为磁罗盘3轴没变，转动的是地磁矢量：把磁罗盘绕各个方面都转了一周，可认为地磁矢量相对磁罗盘的原点画出一个球面

• 当不考虑三轴不垂直引起的误差时，测量值将形成椭圆，椭圆的三个轴长表征尺度因子s，圆心表征偏置因子b。
  

更精确的标定，见吴永亮,王田苗,梁建宏.微小型无人机三轴磁强计现场误差校正方法[J].航空学报,2011,32(02):330-336.

PX4中的磁罗盘标定

1 状态估计简介

1.1 状态估计

• 状态估计（state estimation）根据可获取的量测数据估算动态系统内部状态的方法。对系统的输入和输出进行测量而得到的数据只能反映系统的外部特性，而系统的动态规律需要用内部状态变量（通常无法直接测量，或测量误差较大）来描述。因此状态估计对于了解和控制一个系统具有重要意义。
  • 外部特性：如，想要确定无人机的姿态，陀螺仪测的只是角速度，并不是直接反映无人机的姿态。
  • 很多时候，传感器测量出来的量跟我们想要获得的系统状态量之间有差距
• 小型无人机中，陀螺仪测量角速度比较准，姿态、速度和位置通常都需要多传感器融合进行状态估计。
  • 现在用的最多的状态估计方法是卡尔曼滤波

• 通常无法直接测量：多传感器融合

• 测量误差较大：位置

2 卡尔曼滤波（KF）

阿波罗登月，实现月球导航

2.1 卡尔曼滤波简介

• 高效率的递归滤波器 => 计算简单；递归：不需要记录之前的数据，对内存利用少
• 时域滤波器
• 根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准
• 卡尔曼滤波建立在线性代数和隐马尔可夫模型上
• 要求高斯噪声，经典卡尔曼滤波要求线性模型，扩展卡尔曼滤波和无迹卡尔曼滤波可用于非线性模型
  • 高斯噪声：传感器测量时的噪声等，若不是高斯噪声，估计效果可能不好
  • 庆幸：物理世界中，绝大多数噪声就是高斯噪声（噪声来源复杂）

2.2 隐马尔科夫模型

基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

2.2.1 系统模型

2.2.2 线性系统模型

• Z_k：多传感器融合中，其维度可能很大
• 映射成测量空间：如，状态x是3维，测量空间是1维的，则需要把3维映射到1维状态

[注]

动态模型一般会分为 线性&非线性 、 时变&非时变

上述的模型可以是时变的，只需要知道F1，F2，F3……，只要确保线性即可

2.2.3 卡尔曼滤波器

卡尔曼滤波器的状态由以下两个变量表示：

• 
  ---- k时刻的状态的估计（拿给控制器的量）

• 
  ---- 后验估计误差协方差矩阵，度量估计值的精确程度（通过看误差的概率分布，知道误差有可能多少大）

• 卡尔曼滤波器的操作包括两个阶段：预测与更新。

  • 在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。
  • 在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

• 预测过程按照如下两个递推式进行：

  • 
    （预测状态）
  • 
    （预测估计协方差矩阵）

• 更新过程按照如下五个递推式进行：

  假设观测得到的陀螺仪测量的角速度，怎么更新刚才仅仅依靠状态转移获得的、这一时刻的状态？

  • 
    （测量残差，新息）
  • 
    （测量残差协方差）
  • 
    （最优卡尔曼增益）
  • 
    （更新的状态估计）
  • 
    （更新的协方差估计）

[注]

当测量值的偏差很大，还不如用

2.2.4 新息卡方检验法

解决均值非零问题，如果现在测出来的磁罗盘的误差均值不是零，通过该检验，把这个检验出来，那么这次就不用这个观测

• 测量有效时，新息
  的均值应该是0；若均值不是0，则可以判定为测量无效

• 对
  进行二元假设：

  （1）H0（有效）：

  （2）H1（无效）：

• 观测值有效性检验函数为

  ​

• 式中,
  ，m是观测量的维数。

  • 若
    ，判定无效,
  • 若
    ，判定有效,

• 
  由人为对于观测的信任程度 决定。 越大, 对于受污染观测的利用程度越大, 反之相反。

  • 若你很信任你的无人机，磁罗盘，认为它们有强的抗磁干扰性，则可以把T_d设置大一些
  • 反之，有一点点检测不对劲就不用这个值

• 与EKF2模块中 (innovation^2 / (innovGate^2 * varInnov)) > 1.0 相对应

  • （新息^2 / (阈值^2 * 协方差矩阵)）

3 扩展卡尔曼滤波（EKF）

3.1 扩展卡尔曼滤波器

• 基本卡尔曼滤波器是限制在线性的假设之下。然而，大部分非平凡的的系统都是非线性系统。其中的“非线性性质”可能是伴随存在过程模型中或观测模型中，或者两者兼有之。（都是非线性）
• 在扩展卡尔曼滤波器（Extended Kalman Filter，简称EKF）中状态转换和观测模型不需要是状态的线性函数，可替换为（可微的）函数。
• 同基本卡尔曼滤波器，噪声为加性高斯白噪声

3.2 线性化

• f 和 h 不能直接的应用在协方差中，取而代之的是计算 雅可比矩阵。在每一步中使用当前的估计状态计算雅可比矩阵，这几个矩阵可以用在卡尔曼滤波器的方程中。

  • 雅可比矩阵：局部近似化，一阶泰勒近似

• 这个过程，实质上将非线性的函数在当前估计值处线性化了。这样一来，卡尔曼滤波器的等式为：

  • 预测

• 更新
  

3.3 两轮车模型仿真

• 由运动学模型建立的状态方程

  • 

• 观测方程

  • 

• 令

  • 

• 从而有

  • 

[注1]

所以W_c是陀螺仪所测得的观测量，为什么它放在状态方程中作为输入？

• 不是所有传感器测得值一定是测量方程的测量量，也可能是状态方程的输入量

[注2]

• 对于无人机问题来说，IMU中加速度计、陀螺仪的测量值（加速度、角速度）都是作为状态方程的输入存在的。

• 主要原因：

  1. 从动力学的数学本质上，有这样的原因
  2. 测量速度快，IMU的测量值是无人机所有传感器中测量速度最快，把这两个作为了基准量

4 互补滤波

4.1 线性互补滤波

• 姿态角变化率和陀螺仪的角速度有如下关系

  

• 当多旋翼机动较小时，θ≈0，ϕ≈0，所以上式近似为

  

  [注]

  • 非线性体现在欧拉角的变化率，与角速度前所乘的一项（与状态本身有关）

  • 通过角速度积分得到欧拉角的值（角速度就是欧拉角变化率）

4.1.1 基本思想

• 姿态角可以由加速度计或磁罗盘测量得到，漂移小，但噪声大。
• 另一方面，姿态角也可以通过陀螺仪测得的角速度积分得到，该方法噪声小，但漂移大。
• 互补滤波器的基本思想是利用它们各自的优势，在频域上特征互补，得到更精确的姿态角。

4.1.2 以俯仰角为例

以俯仰角为例推导线性互补滤波器。俯仰角θ的拉氏变换可以表示为（这一步是纯数学变换）

为了估计俯仰角，以上式子的θ需要用传感器测量量替代。

1. 加速度计测量的俯仰角无漂移但噪声大，为了简便，俯仰角测量值建模为

   

2. 陀螺仪的角速度测量会有漂移但噪声小，可以建模为，这里做了近似，认为漂移为常值c

   

3. 线性互补滤波器的标准形式以传递函数方式表示为

   

4. 为了计算机算法实现，需要对其进行离散化

   

5. 通过一阶向后差分法，将s表示为（T_s为采样周期）

   

6. 进一步表示为

   

7. 再把上式转化为差分形式得到

   

• 非线性互补滤波器与线性互补滤波器的大体思想相似

  • 都是利用加速度计和陀螺仪的互补优势
  • 不同之处在于它们不是直接线性相加，而是遵从角速度和角度的非线性关系。

• 定义表示互补滤波器估计输出的姿态旋转矩阵，R_m 代表由加速度计或磁罗盘观测到的姿态旋转矩阵，而表示R_m 与 R_hat 之间的误差，定义为

• 根据非线性互补滤波器的原理，可以按照如下形式进行滤波
  

详见 Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogonal group [J]. IEEE Transactions on Automatic Control, 2008, 53(5): 1203-1218.

5 多采样频率与多延时下姿态估计策略

5.1 实际工程与理论的差别

• 传感器采集信号有延迟（如相机曝光时间）
• 不同传感器，采样频率不同（IMU可能高达1000Hz以上，相机可能只有20Hz）
• 传感器信息处理与传输有延时

这些在理论设计滤波器的时候，都没有考虑，那会不会影响滤波器的稳定性呢？

[注]

• 多采样频率：不同传感器采样频率不同
• 多延时：不同传感器测量延时不同

5.2 姿态传感器采样和延迟效应的建模

• ZOH：零阶保持
• Delay：总延时

5.3 问题描述

• 
  传感器测量得到值
• 
  在这段时间都是一个值
• 连续测量量：认为角速度测量量连续，因为频率高
• 连续估计：得到比较高频的姿态估计

5.4 问题解决

6 PX4中的状态估计

• https://github.com/PX4/ecl
• https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html – 文档
• https://github.com/PX4/ecl/blob/master/EKF/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m