Skip to content

Latest commit

 

History

History
249 lines (174 loc) · 17.8 KB

README.md

File metadata and controls

249 lines (174 loc) · 17.8 KB

Lecture5-Homework

按照不需要转台标定方法中所给出的内参模型及残差模型,推导加速度计对应残差对加速度内参的 雅可比,在开源代码中按新的内参模型 (开源代码中加速度计内参模型是上三角,本课程模型是下 三角)改成解析式求导,并使用课程给定的仿真数据做验证。

雅可比推导

作业中使用了开源代码中的模型进行推导

加速度计安装误差

刻度系数误差

偏置

真实值与加速度计输出值关系为：

其中为真实值， 为加速度计输出值

当地重力矢量为

则有残差函数

残差对加速度计估计参数的偏导为

安装误差取下三角模型

则有

Ceres 实现

// 解析求导
class AnalyticMultiPosAccResidual : public ceres::SizedCostFunction<1, 9> {
  public:
    AnalyticMultiPosAccResidual(const double &g_mag, const Eigen::Matrix<double, 3, 1> &sample)
      : g_mag_(g_mag), sample_(sample) {}
    
    virtual ~AnalyticMultiPosAccResidual() {}

    virtual bool Evaluate(double const* const* params, double *residuals, double **jacobians) const {
        const double Txz = params[0][0];
        const double Txy = params[0][1];
        const double Tyx = params[0][2];
        const double Kx = params[0][3];
        const double Ky = params[0][4];
        const double Kz = params[0][5];
        const double bx = params[0][6];
        const double by = params[0][7];
        const double bz = params[0][8];

        // lower triad model
        Eigen::Matrix<double, 3, 3> T;
        T << 1, 0, 0, 
             Txz, 1, 0, 
             -Txy, Tyx, 1;
        
        Eigen::Matrix3d K = Eigen::Matrix3d::Identity();
        K(0, 0) = Kx;
        K(1, 1) = Ky;
        K(2, 2) = Kz;

        Eigen::Vector3d bias(bx, by, bz);

        Eigen::Vector3d calib_samp = T * K * (sample_.col(0) - bias);

        residuals[0] = g_mag_ - calib_samp.norm();

        if (jacobians != nullptr) {
            if (jacobians[0] != nullptr) {
                Eigen::Vector3d x_xnorm = calib_samp / (calib_samp.norm());
                // 测量值减去bias
                Eigen::Vector3d v = sample_ - bias;
                // calib_samp 对参数的偏导
                Eigen::Matrix<double, 3, 9> J_theta = Eigen::Matrix<double, 3, 9>::Zero();
                J_theta(0, 3) = v(0);
                J_theta(0, 6) = -Kx;
                J_theta(1, 0) = Kx * v(0);
                J_theta(1, 3) = Txz * v(0);
                J_theta(1, 4) = v(1);
                J_theta(1, 6) = -Txz * Kx;
                J_theta(1, 7) = -Ky;
                J_theta(2, 1) = -Kx * v(0);
                J_theta(2, 2) = Ky * v(1);
                J_theta(2, 3) = -Txy * v(0);
                J_theta(2, 4) = Tyx * v(1);
                J_theta(2, 5) = v(2);
                J_theta(2, 6) = Txy * Kx;
                J_theta(2, 7) = -Tyx * Ky;
                J_theta(2, 8) = -Kz;
                Eigen::Matrix<double, 1, 9> Jaco = - x_xnorm.transpose() * J_theta;
                jacobians[0][0] = Jaco(0, 0);
                jacobians[0][1] = Jaco(0, 1);
                jacobians[0][2] = Jaco(0, 2);
                jacobians[0][3] = Jaco(0, 3);
                jacobians[0][4] = Jaco(0, 4);
                jacobians[0][5] = Jaco(0, 5);
                jacobians[0][6] = Jaco(0, 6);
                jacobians[0][7] = Jaco(0, 7);
                jacobians[0][8] = Jaco(0, 8);
            }
        }
        return true;
    }

  private:
    const double g_mag_;
    const Eigen::Matrix< double, 3 , 1> sample_;
};

// 添加参数
ceres::CostFunction* cost_function = new AnalyticMultiPosAccResidual(g_mag_, static_samples[i].data());
problem.AddResidualBlock(cost_function, NULL, acc_calib_params.data());

结果对比

  • 开源代码自动求导标定结果
Accelerometers calibration: Better calibration obtained using threshold multiplier 6 with residual 0.120131
Misalignment Matrix
          1  -0.0033593 -0.00890639
          0           1  -0.0213341
        -0           0           1
Scale Matrix
0.00241278          0          0
        0 0.00242712          0
        0          0 0.00241168
Bias Vector
33124.2
33275.2
32364.4

Accelerometers calibration: inverse scale factors:
414.459
412.01
414.649

Gyroscopes calibration: residual 0.00150696
Misalignment Matrix
        1 0.00593634 0.00111101
0.00808812          1 -0.0535569
0.0253067 -0.0025513          1
Scale Matrix
0.000209295           0           0
          0 0.000209899           0
          0           0 0.000209483
Bias Vector
32777.1
32459.8
32511.8

Gyroscopes calibration: inverse scale factors:
4777.96
4764.21
4773.66
  • 解析求导标定结果
Accelerometers calibration: Better calibration obtained using threshold multiplier 6 with residual 0.120131
Misalignment Matrix
          1          -0           0
-0.00354989           1          -0
-0.00890444  -0.0213032           1
Scale Matrix
0.00241267          0          0
         0 0.00242659          0
         0          0 0.00241232
Bias Vector
33124.2
33275.2
32364.4

Accelerometers calibration: inverse scale factors:
414.478
412.102
414.538

Gyroscopes calibration: residual 0.00150696
Misalignment Matrix
        1 0.00927517 0.00990014
0.00507442          1 -0.0322229
0.0162201 -0.0239393          1
Scale Matrix
0.000209338           0           0
        0 0.000209834           0
        0           0 0.000209664
Bias Vector
32777.1
32459.8
32511.8

Gyroscopes calibration: inverse scale factors:
4776.96
4765.68
4769.53