文章目录

• 原理
• • UKF
  • • Sigma and weight
    • UKF Algorithm
    • UT/UKF/EKF Summary
• cato_code
• • 外围函数
  • • 检测是否为对称矩阵
    • 矩阵的开方根
    • 高斯分布
  • UKF 代码实现
  • • 类
    • 预测
    • 观测更新
    • 点评

原理

UKF

• KF 系列求解：
  • Kalman filter 需要线性模型
  • EKF通过泰勒展开线性化
  • 更好的方式线性化 -> Unscented Transform -> UKF
    • 计算一组（所谓的）sigma 点
    • 从变换和加权的 sigma 点计算高斯
• Unscented Transform
  • 计算一系列的 Sigma 点
  • 每个Sigma点有一个权重
  • 通过非线性函数转换 Sigma 点
  • 权重点计算高斯

Sigma and weight

• Sigma 点
  • 选择 χ[i]{\chi^{[i]}}χ[i]，w[i]{w^{[i]}}w[i] 使得：
    • ∑iw[i]=1{\sum_i w^{[i]} = 1}∑i​w[i]=1
    • μ=∑iw[i]χ[i]{ \mu = \sum_i w^{[i]}\chi^{[i]}}μ=∑i​w[i]χ[i]
    • ∑=∑iw[i](χ[i]−μ)(χ[i]−μ)T{\sum = \sum_i w^{[i]}(\chi^{[i]}-\mu)(\chi^{[i]}-\mu)^T}∑=∑i​w[i](χ[i]−μ)(χ[i]−μ)T
  • 没有唯一的解决方案
  • 如何选择Sigma点
    • 第一个Sigma点也是均值 χ[0]=μ{\chi^[0] = \mu}χ[0]=μ
    • χ[i]=μ+((n+λ)∑)i{\chi^[i] = \mu + (\sqrt{(n+\lambda)\sum})_i}χ[i]=μ+((n+λ)∑​)i​ for i=1,…,n
    • χ[i]=μ−((n+λ)∑)i−n{\chi^[i] = \mu - (\sqrt{(n+\lambda)\sum})_{i-n}}χ[i]=μ−((n+λ)∑​)i−n​ for i=1+n,…,2n
      • 矩阵的开方根
      • nnn 维度
      • λ\lambdaλ 缩放参数
  • 矩阵开方根
    • 定义 SSS，∑=SS\sum=SS∑=SS
    • 通过对角化计算：
      • ∑=VDV−1=(d11⋯0⋮⋱⋮0⋯dnn)=V(d11⋯0⋮⋱⋮0⋯dnn)(d11⋯0⋮⋱⋮0⋯dnn)V−1{\sum=VDV^{-1}= \begin{pmatrix} d_{11} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & d_{nn} \end{pmatrix} = V\begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} V^{-1}}∑=VDV−1=⎝⎜⎛​d11​⋮0​⋯⋱⋯​0⋮dnn​​⎠⎟⎞​=V⎝⎜⎛​d11​​⋮0​⋯⋱⋯​0⋮dnn​​​⎠⎟⎞​⎝⎜⎛​d11​​⋮0​⋯⋱⋯​0⋮dnn​​​⎠⎟⎞​V−1
      • 因此可以定义： S=VD1/2V−1{S=VD^{1/2}V^{-1}}S=VD1/2V−1
      • 因此：SS=(VD1/2V−1)(VD1/2V−1)=VDV−1=ΣSS=(VD^{1/2}V^{-1})(VD^{1/2}V^{-1})=VDV^{-1}=\SigmaSS=(VD1/2V−1)(VD1/2V−1)=VDV−1=Σ
    • Cholesky Matrix 开方根法
      • 矩阵开方根的替代定义：L,∑=LLT{L, \sum=LL^T}L,∑=LLT
      • L,∑{L,\sum}L,∑ 有相同的特征向量
  • Sigma 点和特征向量
    • Sigma 点可以但不必位于 Σ{\Sigma}Σ 的主轴上
      • χ[i]=μ+((n+λ)∑)i{\chi^[i] = \mu + (\sqrt{(n+\lambda)\sum})_i}χ[i]=μ+((n+λ)∑​)i​ for i=1,…,n
      • χ[i]=μ−((n+λ)∑)i−n{\chi^[i] = \mu - (\sqrt{(n+\lambda)\sum})_{i-n}}χ[i]=μ−((n+λ)∑​)i−n​ for i=1+n,…,2n
  • Sigma 点 权重
    • wm[0]=λn+λ{w_m^{[0]}=\frac{\lambda}{n+\lambda}}wm[0]​=n+λλ​
    • wc[0]=wm[0]+(1−α2+β){w_c^{[0]}=w_m^{[0]}+(1-\alpha^2+\beta)}wc[0]​=wm[0]​+(1−α2+β)
    • wc[i]=wm[i]=12(n+λ){w_c^{[i]}=w_m^{[i]} = \frac{1}{2(n+\lambda)}}wc[i]​=wm[i]​=2(n+λ)1​ for i=1,…,2n
    • 其中：
      • wm[i]{w_m^{[i]}}wm[i]​ 用于计算均值，
      • wc[i]{w_c^{[i]}}wc[i]​ 用于计算协方差
    • 右边的为参数
• 恢复高斯
  • 从权重和 点计算高斯
  • μ′=∑i=02nwm[i]g(χ[i]){\mu'=\sum_{i=0}^{2n}w_m^{[i]}\ g({\chi}^{[i]})}μ′=∑i=02n​wm[i]​ g(χ[i])
  • Σ′=∑i=02nwc[i](g(χ[i])−μ′)(g(χ[i])−μ′)T{\Sigma'=\sum_{i=0}^{2n} w_c^{[i]}(g(\chi^{[i]})-\mu')(g(\chi^{[i]})-\mu')^T}Σ′=∑i=02n​wc[i]​(g(χ[i])−μ′)(g(χ[i])−μ′)T

UKF Algorithm

• Sigma points and Weight

  • 点：
    • χ[0]=μ\chi^{[0]}=\muχ[0]=μ
    • χ[i]=μ+((n+λ)∑)i{\chi^[i] = \mu + (\sqrt{(n+\lambda)\sum})_i}χ[i]=μ+((n+λ)∑​)i​ for i=1,…,n
    • χ[i]=μ−((n+λ)∑)i−n{\chi^[i] = \mu - (\sqrt{(n+\lambda)\sum})_{i-n}}χ[i]=μ−((n+λ)∑​)i−n​ for i=1+n,…,2n
  • 权重：
    • wm[0]=λn+λ{w_m^{[0]}=\frac{\lambda}{n+\lambda}}wm[0]​=n+λλ​
    • wc[0]=wm[0]+(1−α2+β){w_c^{[0]}=w_m^{[0]}+(1-\alpha^2+\beta)}wc[0]​=wm[0]​+(1−α2+β)
    • wc[i]=wm[i]=12(n+λ){w_c^{[i]}=w_m^{[i]} = \frac{1}{2(n+\lambda)}}wc[i]​=wm[i]​=2(n+λ)1​ for i=1,…,2n
  • 参数：
    • k≥0k \geq 0k≥0
    • α∈(0,1]\alpha \in (0,1]α∈(0,1] 影响Sigma点与均值的距离
    • λ=α(n+k)−n\lambda =\alpha(n+k)-nλ=α(n+k)−n
    • β=2\beta=2β=2 优化选择为高斯
    • 

• Prediction

  • χt−1=(μt−1,μt−1+(n+λ)∑t−1μt−1−(n+λ)∑t−1){\chi_{t-1}=(\mu_{t-1},\ \ \mu_{t-1}+\sqrt{(n+\lambda)\sum_{t-1}}\ \ \mu_{t-1}-\sqrt{(n+\lambda)\sum_{t-1}})}χt−1​=(μt−1​,  μt−1​+(n+λ)∑t−1​​  μt−1​−(n+λ)∑t−1​​)

  • χˉt∗=g(ut,χt−1){\bar{\chi}_t^* = g(u_t,\chi_{t-1})}χˉ​t∗​=g(ut​,χt−1​)

  • μtˉ=∑i=02nwm[i]χˉt∗[i]{\bar{\mu_t}=\sum_{i=0}^{2n}w_m^{[i]}\bar{\chi}_t^{*[i]}}μt​ˉ​=∑i=02n​wm[i]​χˉ​t∗[i]​

  • Σˉt=∑i=02nwc[i](χˉt∗[i]−μtˉ)(χˉt∗[i]−μtˉ)T+Rt{\bar{\Sigma}_t=\sum_{i=0}^{2n}w_c^{[i]}(\bar{\chi}_t^{*[i]}-\bar{\mu_t})(\bar{\chi}_t^{*[i]}-\bar{\mu_t})^T+R_t}Σˉt​=∑i=02n​wc[i]​(χˉ​t∗[i]​−μt​ˉ​)(χˉ​t∗[i]​−μt​ˉ​)T+Rt​

• Correction

  • χtˉ=(μtˉ,μtˉ+(n+λ)∑t−1,μtˉ−(n+λ)∑t−1){\bar{\chi_{t} }=(\bar{\mu_{t}},\ \ \bar{\mu_{t}}+\sqrt{(n+\lambda)\sum_{t-1}},\ \ \bar{\mu_{t}}-\sqrt{(n+\lambda)\sum_{t-1}})}χt​ˉ​=(μt​ˉ​,  μt​ˉ​+(n+λ)∑t−1​​,  μt​ˉ​−(n+λ)∑t−1​​)
  • ztˉ=h(χtˉ){\bar{z_t}=h(\bar{\chi_t})}zt​ˉ​=h(χt​ˉ​)
  • zt^=∑i=02nwm[i]zˉt[i]{\hat{z_t}=\sum_{i=0}^{2n}w_m^{[i]}\bar{z}_t^{[i]}}zt​^​=∑i=02n​wm[i]​zˉt[i]​
  • St=∑i=02nwc[i](χˉt[i]−μˉt)(χˉt[i]−μˉt)T{S_t=\sum_{i=0}^{2n}w_c^{[i]}(\bar{\chi}_t^{[i]}-\bar{\mu}_t)(\bar{\chi}_t^{[i]}-\bar{\mu}_t)^T}St​=∑i=02n​wc[i]​(χˉ​t[i]​−μˉ​t​)(χˉ​t[i]​−μˉ​t​)T
  • Kt=∑ˉtx,zSt−1{K_t = \bar{\sum}_t^{x,z}S_t^{-1}}Kt​=∑ˉ​tx,z​St−1​
  • ∑t=∑ˉt−KtStKtT{\sum_t =\bar{\sum}_t - K_tS_tK_t^T}∑t​=∑ˉ​t​−Kt​St​KtT​

UT/UKF/EKF Summary

• UT/UKF
  • 无迹卡尔曼作为线性化的替代方案
  • UT 是比泰勒展开更好的近似值
  • UT 使用 sigma 点传播
  • UT中的自由参数
  • UKF 在预测和校正步骤中使用 UT
• UKF VS EKF
  • 线性模型的结果与 EKF 相同
  • 非线性模型比 EKF 更好的近似
  • 差异通常“有点小”
  • UKF 不需要雅可比行列式
  • 相同的复杂度类
  • 比 EKF 稍慢
  • 仍然受限于高斯分布

cato_code

外围函数

检测是否为对称矩阵

检查A是否是对称矩阵,A减去A的转置~=0

• ∣∣(A−AT)∣∣2<1e−5{||(A-A^T)||_2 < 1e-5}∣∣(A−AT)∣∣2​<1e−5

// Checks if 'A' is a symmetric matrix.
template 
void CheckSymmetric(const Eigen::Matrix& 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;
}

矩阵的开方根

本代码使用的为上述中 对角化的方法：

• 定义 SSS，∑=SS\sum=SS∑=SS
• 通过对角化计算：
  • ∑=VDV−1=(d11⋯0⋮⋱⋮0⋯dnn)=V(d11⋯0⋮⋱⋮0⋯dnn)(d11⋯0⋮⋱⋮0⋯dnn)V−1{\sum=VDV^{-1}= \begin{pmatrix} d_{11} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & d_{nn} \end{pmatrix} = V\begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} \begin{pmatrix} \sqrt{d_{11}} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \sqrt{d_{nn}} \end{pmatrix} V^{-1}}∑=VDV−1=⎝⎜⎛​d11​⋮0​⋯⋱⋯​0⋮dnn​​⎠⎟⎞​=V⎝⎜⎛​d11​​⋮0​⋯⋱⋯​0⋮dnn​​​⎠⎟⎞​⎝⎜⎛​d11​​⋮0​⋯⋱⋯​0⋮dnn​​​⎠⎟⎞​V−1
  • 因此可以定义： S=VD1/2V−1{S=VD^{1/2}V^{-1}}S=VD1/2V−1

代码实现：

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

• template 
  Eigen::Matrix MatrixSqrt(const Eigen::Matrix& A) {CheckSymmetric(A);//必须是对称矩阵Eigen::SelfAdjointEigenSolver>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::Zero()).cwiseSqrt().asDiagonal() *adjoint_eigen_solver.eigenvectors().transpose();
  }
  

高斯分布

高斯分布

• 均值 ： N×1N\times 1N×1

• 协方差： N×NN\times NN×N

• /*
  高斯分布类
  构造函数是N*1的均值矩阵和N*N的协方差矩阵*/
  template 
  class GaussianDistribution {public:GaussianDistribution(const Eigen::Matrix& mean,const Eigen::Matrix& covariance): mean_(mean), covariance_(covariance) {}const Eigen::Matrix& GetMean() const { return mean_; }const Eigen::Matrix& GetCovariance() const { return covariance_; }private:Eigen::Matrix mean_;       //N*1，均值Eigen::Matrix covariance_; //N*N。协方差
  };
  

高斯 相加

• 均值：均值+均值

• 协方差：协方差+协方差

• /*高斯分布性质:
  https://zh.wikipedia.org/wiki/%E6%AD%A3%E6%80%81%E5%88%86%E5%B8%83
  */
  template 
  GaussianDistribution operator+(const GaussianDistribution& lhs,const GaussianDistribution& rhs) {return GaussianDistribution(lhs.GetMean() + rhs.GetMean(),lhs.GetCovariance() + rhs.GetCovariance());
  }
  

高斯 相乘

• 均值： (N×M)⋅(M×1)=(N×1)(N\times M) \cdot (M \times 1) = (N \times 1)(N×M)⋅(M×1)=(N×1)

• 协方差：(N×M)⋅(M×M)⋅(M×N)=(N×N)(N\times M) \cdot (M \times M) \cdot (M \times N) = (N \times N)(N×M)⋅(M×M)⋅(M×N)=(N×N)

• template 
  GaussianDistribution operator*(const Eigen::Matrix& lhs,const GaussianDistribution& rhs) {return GaussianDistribution(lhs * rhs.GetMean(),                          // N*M || M*1 -> N*1lhs * rhs.GetCovariance() * lhs.transpose()); // N*M ||M*M || M*N ->  N*N
  }
  

UKF 代码实现

卡尔曼滤波器的操作包括两个阶段：

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

类

template 
class UnscentedKalmanFilter {
public:using StateType = Eigen::Matrix;           //状态矩阵N*1using StateCovarianceType = Eigen::Matrix; //协方差矩阵N*Nexplicit UnscentedKalmanFilter(const GaussianDistribution& initial_belief,                 //参数1std::function  //参数2add_delta = [](const StateType& state,const StateType& delta) { return state + delta; },std::function //参数3compute_delta =[](const StateType& origin, const StateType& target) {return target - origin;}): belief_(initial_belief),add_delta_(add_delta),compute_delta_(compute_delta) {}/*** @brief Predict 预测step。使用卡尔曼滤波对当前状态进行预测。* @param g  控制必须由函数 g 隐式添加，该函数也进行状态转换。* @param epsilon  “epsilon”是控制噪声和模型噪声的加法组合。*/void Predict(std::function g,const GaussianDistribution& epsilon);/*** @brief Observe  观测步骤* @param h   'h' 将状态转换为应该为零的观察值，传感器读数应该已经包含在这个函数中。* @param delta  'delta' 是测量噪声，必须具有零均值。*/template void Observe(std::function(const StateType&)> h,const GaussianDistribution& delta)private://计算带权重的偏差，权重事先算好了StateType ComputeWeightedError(const StateType& mean_estimate,const std::vector& states) {StateType weighted_error =kMeanWeight0 * compute_delta_(mean_estimate, states[0]);for (int i = 1; i != 2 * N + 1; ++i) {weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);}return weighted_error;}//计算均值。基于权重误差评判计算StateType ComputeMean(const std::vector& states) {CHECK_EQ(states.size(), 2 * N + 1);StateType current_estimate = states[0];StateType weighted_error = ComputeWeightedError(current_estimate, states);int iterations = 0;while (weighted_error.norm() > 1e-9) {double step_size = 1.;while (true) {const StateType next_estimate =add_delta_(current_estimate, step_size * weighted_error);const StateType next_error =ComputeWeightedError(next_estimate, states);if (next_error.norm() < weighted_error.norm()) {current_estimate = next_estimate;weighted_error = next_error;break;}step_size *= 0.5;CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";}++iterations;CHECK_LT(iterations, 20) << "Too many iterations.";}return current_estimate;}// 常值确定参数   sqr=a*a, OuterProduct=V*V^T N*1 || 1*N -> 外积,N*Nconstexpr static FloatType kAlpha = 1e-3;constexpr static FloatType kKappa = 0.;constexpr static FloatType kBeta = 2.;constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);constexpr static FloatType kCovWeight0 =kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));constexpr static FloatType kCovWeightI = kMeanWeightI;// 成员变量//1),N*1矩阵,对N个变量的估计GaussianDistribution belief_; //2),add_delta_，加法操作const std::functionadd_delta_;//3),compute_delta_，计算偏差操作const std::functioncompute_delta_;
};//外部声明。感觉这儿没啥用
template 
constexpr FloatType UnscentedKalmanFilter::kAlpha;
template 
constexpr FloatType UnscentedKalmanFilter::kKappa;
template 
constexpr FloatType UnscentedKalmanFilter::kBeta;
template 
constexpr FloatType UnscentedKalmanFilter::kLambda;
template 
constexpr FloatType UnscentedKalmanFilter::kMeanWeight0;
template 
constexpr FloatType UnscentedKalmanFilter::kCovWeight0;
template 
constexpr FloatType UnscentedKalmanFilter::kMeanWeightI;
template 
constexpr FloatType UnscentedKalmanFilter::kCovWeightI;

预测

步骤：

• 1、判断协方差 为对称矩阵
  2、取当前状态的均值和 协方差开方根  （生成Sigma点要用）
  3、计算状态转移矩阵，均值+协方差均值直接权重相加，然后基于误差在一定范围得到`最优均值`协方差 直接基于 `最优均值` 重新计算
  4、更新状态  均值+协方差
  

代码：

void Predict(std::function g,const GaussianDistribution& epsilon) {/// 1. 协方差是对称矩阵CheckSymmetric(epsilon.GetCovariance());// Get the state mean and matrix root of its covariance./// 2. 取当前状态的均值+协方差开方根const StateType& mu = belief_.GetMean();const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());//N*N// 由于UKF，采用点2N+1std::vector Y;//需要计算的状态矩阵，N*1矩阵。Y.reserve(2 * N + 1);//公式：p65/// 3. 计算状态转移矩阵，均值+协方差Y.emplace_back(g(mu));//状态转移方程,公式p65:3.68,p70:3const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);for (int i = 0; i < N; ++i) {// Order does not matter here as all have the same weights in the// summation later on anyways.Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));}// 得到最优 均值 (误差小于阈值)const StateType new_mu = ComputeMean(Y);// 基于最新阈值重新计算新的协方差StateCovarianceType new_sigma =kCovWeight0 * OuterProduct(compute_delta_(new_mu, Y[0]));for (int i = 0; i < N; ++i) {new_sigma += kCovWeightI * OuterProduct(compute_delta_(new_mu, Y[2 * i + 1]));new_sigma += kCovWeightI * OuterProduct(compute_delta_(new_mu, Y[2 * i + 2]));}CheckSymmetric(new_sigma);//更新状态  均值+协方差belief_ = GaussianDistribution(new_mu, new_sigma) + epsilon;
}

观测更新

步骤：

1、判断 观测均值和协方差，均值0附近，协方差对称矩阵
2、取当前状态的均值和 协方差开方根  （生成Sigma点要用）
3、计算0均值sigma点和Z，得出 总体观测均值 ，协方差均值经转移方程后直接权重相加，得到`最新均值`协方差 直接基于 `最新均值` 重新计算，同时加上观测自身的协方差
4、计算 (x,z)的sigma。x 即sigma，z (z_t -z_u)
5、得到 K_t
6、更新 协方差 。原协方差-ksk^T
7、更新状态  均值+协方差

代码实现：

/*** @brief Observe  观测步骤* @param h   'h' 将状态转换为应该为零的观察值，传感器读数应该已经包含在这个函数中。* @param delta  'delta' 是测量噪声，必须具有零均值。*/
void Observe(std::function(const StateType&)> h,const GaussianDistribution& delta) {/// 1. 观测均值0附近，观测协方差是对称矩阵CheckSymmetric(delta.GetCovariance());// We expect zero mean delta.CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);/// 2. 取当前状态的均值+协方差开方根// Get the state mean and matrix root of its covariance.const StateType& mu = belief_.GetMean();const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());// As in Kraft's paper, we compute W containing the zero-mean sigma points,// since this is all we need.// 计算0均值的sigma点，及转移后的sigma值std::vector W;W.reserve(2 * N + 1);W.emplace_back(StateType::Zero());std::vector> Z;Z.reserve(2 * N + 1);Z.emplace_back(h(mu));/// 3. 计算0均值sigma点和Z，得出 总体观测均值Eigen::Matrix z_hat = kMeanWeight0 * Z[0];const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);for (int i = 0; i < N; ++i) {// Order does not matter here as all have the same weights in the// summation later on anyways.W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));Z.emplace_back(h(add_delta_(mu, W.back())));W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));Z.emplace_back(h(add_delta_(mu, W.back())));z_hat += kMeanWeightI * Z[2 * i + 1];z_hat += kMeanWeightI * Z[2 * i + 2];}/// 4. 得出总体观测的 协方差(递推协方差+自身协方差)Eigen::Matrix S =kCovWeight0 * OuterProduct(Z[0] - z_hat);for (int i = 0; i < N; ++i) {S += kCovWeightI * OuterProduct(Z[2 * i + 1] - z_hat);S += kCovWeightI * OuterProduct(Z[2 * i + 2] - z_hat);}CheckSymmetric(S); // 递推协方差 对称矩阵S += delta.GetCovariance();/// 5. 计算Sigma_x,zEigen::Matrix sigma_bar_xz =kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();for (int i = 0; i < N; ++i) {sigma_bar_xz +=kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();sigma_bar_xz +=kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();}/// 6. 计算 K_t，更新协方差const Eigen::Matrix kalman_gain =sigma_bar_xz * S.inverse();const StateCovarianceType new_sigma =belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();CheckSymmetric(new_sigma);/// 7. 更新 状态方程belief_ = GaussianDistribution(add_delta_(mu, kalman_gain * -z_hat), new_sigma);
}

点评

• 预测、更新的状态方程都可以动态给出
• 代码结构清晰，属实不错