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

【cartographer源码分析】系列的第五部分【kalman_filter】。

现总结kalman_filter文件夹涉及到的主要功能：

【1】kalman_filter/gaussian_distribution.h

GaussianDistribution定义了多个变量的高斯分布。构造函数是N1的均值矩阵和NN的协方差矩阵

• 数据成员:
  • Eigen::Matrix<T, N, 1> mean_; //N*1，均值
  • Eigen::Matrix<T, N, N> covariance_; //N*N。协方差
• 构造函数
• GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean, const Eigen::Matrix<T, N, N>& covariance) 初始化均值和协方差
• 全局函数
  • 重载"+"加号操作符 * GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs, const GaussianDistribution<T, N>& rhs) 高斯+高斯=对应均值+均值,对应协方差+协方差 返回值：新高斯对象 .
• 重载乘法运算符 * GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs, const GaussianDistribution<T, M>& rhs) 1,矩阵NM 2,高斯分布MM 返回值:高斯分布：N*N

[2] unscented_kalman_filter.h

UnscentedKalmanFilter类是根据《Probabilistic Robotics》实现的无损卡尔曼滤波的算法，并且根据论文 A Quaternion-based Unscented Kalman Filter for Orientation，Kraft ,E.中的算法，扩展到了处理非线性噪声和传感器。卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

• 全局函数:
• constexpr FloatType sqr(FloatType a) 求平方
• Eigen::Matrix<FloatType, N, N> OuterProduct( const Eigen::Matrix<FloatType, N, 1>& v) ；求 N1× 1N -> 外积,N*N
• void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) ；检查A是否是对称矩阵,A减去A的转置~=0
• Eigen::Matrix<FloatType, N, N> MatrixSqrt(const Eigen::Matrix<FloatType, N, N>& A) 返回对称半正定矩阵的平方根B,M=B*B

. UnscentedKalmanFilter类