cartographer源码分析(43)-kalman_filterunscented_kalman_filter.h

源码可在https://github.com/learnmoreonce/SLAM 下载

文件:unscented_kalman_filter.h


#ifndef CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_

#include <algorithm>
#include <cmath>
#include <functional>
#include <vector>

#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "glog/logging.h"
/*
无损卡尔曼滤波,去芳香卡尔曼滤波https://en.wikipedia.org/wiki/Kalman_filter
*/
namespace cartographer {
namespace kalman_filter {

//平方,常量表达式
template <typename FloatType>
constexpr FloatType sqr(FloatType a) {
  return a * a;
}

// N*1 || 1*N -> 外积,N*N
template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> OuterProduct( 
    const Eigen::Matrix<FloatType, N, 1>& v) {
  return v * v.transpose();
}

// Checks if 'A' is a symmetric matrix.检查A是否是对称矩阵,A减去A的转置~=0
template <typename FloatType, int N>
void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
  // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
  // symmetric.

  //The NaN values are used to identify undefined or non-representable values for floating-point elements, 
  //such as the square root of negative numbers or the result of 0/0.

  const FloatType norm = (A - A.transpose()).norm();
  CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
      << "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
      << A;
}

/*
https://zh.wikipedia.org/wiki/%E6%AD%A3%E5%AE%9A%E7%9F%A9%E9%98%B5

几个术语: Av=λv.  λ 为一标量，称为 v 对应的特征值。也称 v 为特征值 λ 对应的特征向量。
eigendecomposition,特征分解,谱分解,是将矩阵分解为由其特征值和特征向量表示的矩阵之积的方法。
需要注意只有对可对角化矩阵才可以施以特征分解。

eigenvalues 特征值,
eigenvectors 特征向量 


返回对称半正定矩阵的平方根B,M=B*B
*/
// Returns the matrix square root of a symmetric positive semidefinite matrix.
template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> MatrixSqrt(
    const Eigen::Matrix<FloatType, N, N>& A) {
  CheckSymmetric(A);//必须是对称矩阵

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
      adjoint_eigen_solver((A + A.transpose()) / 2.);
      //A==A的转置，构造特征分解对象。

  const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();//特征值
  CHECK_GT(eigenvalues.minCoeff(), -1e-5)
      << "MatrixSqrt failed with negative eigenvalues: "
      << eigenvalues.transpose();
//Cholesky分解：把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。
  return adjoint_eigen_solver.eigenvectors() *
         adjoint_eigen_solver.eigenvalues()
             .cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
             .cwiseSqrt()
             .asDiagonal() *
         adjoint_eigen_solver.eigenvectors().transpose();
}

/*
无损卡尔曼滤波
对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。
卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。
这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），
也可以是对过去位置的估计（插值或平滑）。

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，
因此不需要记录观测或者估计的历史信息。

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。
在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。


UnscentedKalmanFilter类是根据《Probabilistic Robotics》实现的卡尔曼滤波类，
并且扩展到了处理非线性噪声和传感器。
数据成员:
1),add_delta_
2),compute_delta_

成员函数：
1),Predict():预测
*/

/*
Implementation of a Kalman filter. We follow the nomenclature (过程)from
Thrun, S. et al., Probabilistic Robotics, 2006.
Extended to handle non-additive noise/sensors inspired by Kraft, E., A
Quaternion-based Unscented Kalman Filter for Orientation Tracking.

*/
template <typename FloatType, int N>
class UnscentedKalmanFilter {
 public:
  using StateType = Eigen::Matrix<FloatType, N, 1>;