ROS中EKF（扩展卡尔曼跟踪）的使用

最新推荐文章于 2023-07-17 16:02:35 发布

刀么克瑟拉莫

最新推荐文章于 2023-07-17 16:02:35 发布

阅读量2.4k

点赞数

分类专栏： ROS

本文链接：https://blog.csdn.net/random_repick/article/details/104020187

版权

0.简版（更新于2020年最后一天）

#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include <bfl/filter/extendedkalmanfilter.h>
#include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <bfl/pdf/linearanalyticconditionalgaussian.h>
#include <fstream>
#include <iostream>

using namespace std;
using namespace BFL;
using namespace MatrixWrapper;

class MYEKF
{
   
public:
    BFL::Gaussian prior_;
    BFL::ExtendedKalmanFilter *filter_;
    BFL::LinearAnalyticConditionalGaussian *sys_pdf_;
    BFL::LinearAnalyticSystemModelGaussianUncertainty *sys_model_;
    BFL::LinearAnalyticConditionalGaussian *meas_pdf_;
    BFL::LinearAnalyticMeasurementModelGaussianUncertainty *meas_model_;
    MatrixWrapper::Matrix sys_matrix_;
    MatrixWrapper::SymmetricMatrix sys_sigma_;
    float time_;

    MYEKF() : filter_(NULL),
              sys_pdf_(NULL),
              sys_model_(NULL),
              meas_pdf_(NULL),
              meas_model_(NULL),
              sys_matrix_(6, 6) {
   }

    void Init(float x, float y, float t)
    {
   
        sys_matrix_ = 0;
        for (unsigned int i = 1; i <= 3; i++)
        {
   
            sys_matrix_(i, i) = 1;
            sys_matrix_(i + 3, i + 3) = 0.9;
        }
        // 噪声的u和sigma，sys_noise即Q
        ColumnVector sys_mu(6);
        sys_mu = 0;
        sys_sigma_ = SymmetricMatrix(6);
        sys_sigma_ = 0;
        for (unsigned int i = 0; i < 3; i++)
        {
   
            sys_sigma_(i + 1, i + 1) = pow(0.05, 2);
            sys_sigma_(i + 4, i + 4) = pow(1.0, 2);
        }
        Gaussian sys_noise(sys_mu, sys_sigma_);
        //
        sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);
        sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);

        // =============================create meas model
        // meas_matrix即H
        Matrix meas_matrix(3, 6);
        meas_matrix = 0;
        for (unsigned int i = 1; i <= 3; i++)
            meas_matrix(i, i) = 1;
        // 噪声的u和sigma，meas_noise即R
        ColumnVector meas_mu(3);
        meas_mu = 0;
        SymmetricMatrix meas_sigma(3);
        meas_sigma = 0;
        for (unsigned int i = 0; i < 3; i++)
            meas_sigma(i + 1, i + 1) = 0;
        Gaussian meas_noise(meas_mu, meas_sigma);
        //
        meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
        meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
        // first point
        ColumnVector mu_vec(6);
        SymmetricMatrix sigma_vec(6);
        sigma_vec = 0;
        mu_vec(1) = x;
        mu_vec(2) = y;
        mu_vec(3) = 0;
        for (unsigned int i = 0; i < 3; i++)
        {
   
            // mu_vec(i + 1) = pos[i];
            mu_vec(i + 4) = 0.0;
            sigma_vec(i + 1, i + 1) = pow(0.1, 2);
            sigma_vec(i + 4, i + 4) = pow(0.0000001, 2);
        }
        prior_ = Gaussian(mu_vec, sigma_vec);
        filter_ = new ExtendedKalmanFilter(&prior_);
        time_ = t;
    }
    bool Predict(float t)
    {
   
        // set F
        for (unsigned int i = 1; i <= 3; i++)
            sys_matrix_(i, i + 3) = t - time_;
        sys_pdf_->MatrixSet(0, sys_matrix_);

        // scale system noise for dt
        sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(t - time_, 2));
        time_ = t;
        // update filter
        return filter_->Update(sys_model_);
    }
    bool Update(float x, float y)
    {
   
        ColumnVector meas_vec(3);
        // for (unsigned int i = 0; i < 3; i++)
        //     meas_vec(i + 1) = p[i];
        meas_vec(1) = x;
        meas_vec(