Ⅲ定位算法

• 乔利斯基（Cholesky）分解

  • 前提：正定对称阵
  • 正定对称阵可唯一分解为下三角矩阵和上三角矩阵112
  • 作用：简化矩阵平方根操作

• 基于协方差矩阵平方根推导卡尔曼滤波更新方程

  • H:观测矩阵、R:观测误差

    

    • 关键：建立了信息协方差S
  • 协方差更新方程更新

    

• 观测到landmark，(ρ，θ)时，环境观测方程及观测Jacobian求解

  

• UKF

  • 通过UT变换实现线性化
    • Step1：采样（2N+1）个Sigma点

      

    • Step2：计算Sigma点权重

      

    • Step3：对Sigma点进行传播

      

    • Step4：均值和协方差计算

      

  • UKF步骤

    

    • 预测

      

      • 关键状态增广
    • 更新

      

• 粒子滤波

  • 含义：

    • 用离散权重粒子集合表征后验分布

  • 重要性采样

    • 原理：
      • 从某个已知密度分布的“重要性函数”中采样
    • 步骤：
      • 已知g密度，未知f密度，求f期望：
        • 从g密度中抽样样本
        • 样本均值为g期望
        • 样本权重计算

          

        • 样本权重均值为f期望

  • 重采样

    • 需要重采样原因：
      • 随着时间迭代粒子传播，粒子集逐步发散、退化，大多数粒子权重为0，有效粒子少，滤波器性能下降。
    • 重采样算法原理
      • 复制高权重样本，去除低权重样本，保持样本总数不变
      • w = f/g
    • 粒子滤波实施流程

      

      • 预测
        • 系统状态传播
        • 随机采样并计算权重
      • 权重归一化
      • 重采样

• 各滤波方法优缺点

  • EKF

    • 优点
      • 应用范围广
      • 计算效率高
    • 缺点
      • 泰勒展开具有局部性，仅适用于小误差协方差
      • 仅可解决初始状态已知、单模高斯后验分布
      • 需要求解Jacobian

  • UKF

    • 优点
      • 精度高，理论上精度可达三阶泰勒展开。当误差大时，测量更准确。
      • 无需求解Jacobian
    • 缺点
      • 计算量大，维度大时不适用

  • PF(粒子滤波)

    • 优点
      • 可解决多模高斯分布问题
      • 非高斯后验分布问题
    • 缺点
      • 不适合高维状态估计问题
      • 计算量大