slam入门 | 卡尔曼滤波器的C++实现

于 2024-04-14 21:09:53 发布
阅读量1.2k 收藏 23
点赞数 24
文章标签： 算法 机器学习 人工智能 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_58102931/article/details/137754320
版权

一、关于卡尔曼滤波器

1.状态估计问题一般分为线性/分线性，高斯/非高斯，批量/递归，而卡尔曼滤波器是一种递归优化方法，是线性高斯系统下的最优无偏估计。

2.卡尔曼滤波器需要给定初始值{x,P}，即给定初始值后还需要其协方差矩阵。

3.卡尔曼滤波器有很多推导方式，一般可以由MAP或贝叶斯推断推导。

二、环境

CMake

Eigen

opencv(只是利用给定随机值的函数)

vector

yaml-cpp

三、C++代码

3.1 建立一个KalmanFilter库，这里借鉴了博主

https://blog.csdn.net/shuoyueqishilove/article/details/81713142?ops_request_misc=&request_id=&biz_id=102&utm_term=C%20++%20kalman%E6%BB%A4%E6%B3%A2&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-81713142.142^v100^pc_search_result_base9&spm=1018.2226.3001.4187icon-default.png?t=N7T8https://blog.csdn.net/shuoyueqishilove/article/details/81713142?ops_request_misc=&request_id=&biz_id=102&utm_term=C%20++%20kalman%E6%BB%A4%E6%B3%A2&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-81713142.142^v100^pc_search_result_base9&spm=1018.2226.3001.4187

using namespace std;
using namespace Eigen;

KalmanFilter::KalmanFilter(int stateSize_, int measSize_, int uSize_) : stateSize(stateSize_), measSize(measSize_),
                                                                        uSize(uSize_) {
    if (stateSize == 0 || measSize == 0) {
        cout << "Error" << endl;
    }

    x.resize(stateSize);
    x.setZero();

    A.resize(stateSize, stateSize);
    A.setIdentity();

    u.resize(uSize);
    u.setZero();

    B.resize(stateSize, uSize);
    B.setZero();

    P.resize(stateSize, stateSize);
    P.setIdentity();

    H.resize(measSize, stateSize);
    H.setZero();

    z.resize(uSize);
    z.setZero();

    Q.resize(stateSize, stateSize);
    Q.setIdentity();

    R.resize(measSize, measSize);
    R.setIdentity();
}

void KalmanFilter::init(VectorXd &x_, MatrixXd &P_, MatrixXd &R_, MatrixXd &Q_) {
    x = x_;
    P = P_;
    R = R_;
    Q = Q_;
}

VectorXd KalmanFilter::predict(MatrixXd &A_, MatrixXd &B_, VectorXd &u_) {
    A = A_;
    B = B_;
    u = u_;
    x = A * x + B * u;
    MatrixXd A_t = A.transpose();
    P = A * P * A_t + Q;
    return x;
}

VectorXd KalmanFilter::update(Eigen::MatrixXd &H_, Eigen::VectorXd &z_meas) {
    H = H_;
    MatrixXd temp1, temp2, H_t;
    H_t = H.transpose();
    temp1 = H * P * H_t;
    temp2 = temp1.inverse();
    MatrixXd K = P * H_t * temp2;
    z = H * x;
    x = x + K * (z_meas - z);
    MatrixXd I = MatrixXd::Identity(stateSize, stateSize);
    P = (I - K * H) * P;
    return x;
}

3.2 数学库

double sample(double x0, double v0, double acc, double t) {
    return x0 + v0 * t + 1 / 2 * acc * t * t;
}

3.3 主代码

using namespace std;
using namespace Eigen;

int main(int argc, char *argv[]) {

    // 读取config文件
    if (argc != 2) {
        cout << "Error,No configfile" << endl;
    }

    YAML::Node config = YAML::LoadFile(argv[1]);
    auto T = config["T"].as<double>();
    auto N = config["N"].as<int>();

    // 导出文件
    ofstream fout;
    fout.open(config["out_path"].as<string>());

    // 设置初始值，u，z的标准差
    double ax_sigma = 1.0;
    double ay_sigma = 1.0;
    double zx_sigma = 0.01;
    double zy_sigma = 0.01;

    // 设置x，u，z矩阵纬度
    auto stateSize = config["stateSize"].as<int>();
    auto measSize = config["measSize"].as<int>();
    auto uSize = config["uSize"].as<int>();

    // 构建kf类
    KalmanFilter kf(stateSize, measSize, uSize);

    // 初始化矩阵和向量
    MatrixXd A(stateSize, stateSize);
    MatrixXd B(stateSize, uSize);
    VectorXd u(uSize);
    VectorXd x(stateSize);
    VectorXd z(measSize);
    MatrixXd H(measSize, stateSize);
    MatrixXd P(stateSize, stateSize);
    MatrixXd Q(stateSize, stateSize);
    MatrixXd R(measSize, measSize);

    // 设置矩阵值
    const YAML::Node &matrix_A = config["A"];
    const YAML::Node &matrix_B = config["B"];
    const YAML::Node &matrix_H = config["H"];
    const YAML::Node &matrix_Q = config["Q"];
    const YAML::Node &matrix_R = config["R"];
    const YAML::Node &matrix_x = config["x"];
    const YAML::Node &matrix_P = config["P"];

    int index = 0;
    for (int row = 0; row < stateSize; row++) {
        for (int col = 0; col < stateSize; col++) {
            A(row, col) = matrix_A[index++].as<double>();
        }
    }

//    A << 1, 0, T, 0,
//            0, 1, 0, T,
//            0, 0, 1, 0,
//            0, 0, 0, 1;

    index = 0;
    for (int row = 0; row < stateSize; row++) {
        for (int col = 0; col < uSize; col++) {
            B(row, col) = matrix_B[index++].as<double>();
        }
    }

//    B << 1 / 2 * T * T, 0,
//            0, 1 / 2 * T * T,
//            T, 0,
//            0, T;

    index = 0;
    for (int row = 0; row < measSize; row++) {
        for (int col = 0; col < stateSize; col++) {
            H(row, col) = matrix_H[index++].as<double>();
        }
    }

//    H << 1, 0, 0, 0,
//            0, 1, 0, 0;

    index = 0;
    for (int row = 0; row < stateSize; row++) {
        for (int col = 0; col < stateSize; col++) {
            P(row, col) = matrix_P[index++].as<double>();
        }
    }

//    P << 1, 0, 0, 0,
//            0, 1, 0, 0,
//            0, 0, 1, 0,
//            0, 0, 0, 1;

    index = 0;
    for (int row = 0; row < stateSize; row++) {
        for (int col = 0; col < stateSize; col++) {
            Q(row, col) = matrix_Q[index++].as<double>();
        }
    }

//    Q << 1 / 4 * ax_sigma * ax_sigma, 0, 0, 0,
//            0, 1 / 4 * ay_sigma * ay_sigma, 0, 0,
//            0, 0, ax_sigma * ax_sigma, 0,
//            0, 0, 0, ay_sigma * ay_sigma;

    index = 0;
    for (int row = 0; row < measSize; row++) {
        for (int col = 0; col < measSize; col++) {
            R(row, col) = matrix_R[index++].as<double>();
        }
    }

//    R << zx_sigma * zx_sigma, 0,
//            0, zy_sigma * zy_sigma;

    // 设置x0的值
    index = 0;
    for (int row = 0; row < stateSize; row++) {
        x(row) = matrix_x[index++].as<double>();
    }

//    x << 0,
//            0,
//            0,
//            0;

    // 构建向量u,创建随机数
    vector<double> ax, ay, rax, ray;
    cv::RNG rng;
    random_device rd1;
    random_device rd2;
    mt19937 gen1(rd1());
    mt19937 gen2(rd2());
    uniform_real_distribution<> dis(0.0, 5.0);
    for (int i = 0; i < N; i++) {
        rax.push_back(dis(gen1));
        ray.push_back(dis(gen2));
        ax.push_back(rax[i] + rng.gaussian(ax_sigma * ax_sigma));
        ay.push_back(ray[i] + rng.gaussian(ay_sigma * ay_sigma));
    }

    // 设置真值
    vector<double> rx, ry, rvx, rvy;
    for (int i = 0; i < N; i++) {
        if (i == 0) {
            rx.push_back(sample(0, 0, rax[i], T));
            ry.push_back(sample(0, 0, ray[i], T));
            rvx.push_back(rax[i] * T);
            rvy.push_back(ray[i] * T);
            continue;
        }
        rx.push_back(sample(rx[i-1], rvx[i-1], rax[i], T));
        ry.push_back(sample(ry[i-1], rvy[i-1], ray[i], T));
        rvx.push_back(rvx[i - 1] + ax[i] * T);
        rvy.push_back(rvy[i - 1] + ay[i] * T);
    }

    // 构建向量z
    vector<double> zx, zy;
    vector<double> vx, vy;
    for (int i = 0; i < N; i++) {
        zx.push_back(rx[i] + rng.gaussian(zx_sigma * zy_sigma));
        zy.push_back(ry[i] + rng.gaussian(zy_sigma * zy_sigma));
    }

    // 卡尔曼滤波过程
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();// 记录时间函数，开始计时
    VectorXd out;
    fout << "x" << "\t\t" << "y" << "\t\t" << "vx" << "\t\t" << "vy" << "\t\t" << "x_u" << "\t\t" << "y_u" << "\t\t"
         << "vx_u" << "\t\t" << "vy_u" << "\t\t" << "rx" << "\t\t" << "ry" << "\t\t" << endl;
    for (int i = 0; i < N; i++) {
        if (i == 0) {
            kf.init(x, P, R, Q);
        }
        // 先验
        u << ax[i],
                ay[i];
        out = kf.predict(A, B, u);
        fout << out(0) << "\t\t" << out(1) << "\t\t" << out(2) << "\t\t" << out(3) << "\t\t";

        // 后验
        z << zx[i],
                zy[i];
        out = kf.update(H, z);
        fout << out(0) << "\t\t" << out(1) << "\t\t" << out(2) << "\t\t" << out(3) << "\t\t" << rx[i] << "\t\t"
             << ry[i] << endl;
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();//结束计时
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << "seconds." << endl;

    /*
    // 打印随机数据
    fout<<"ax"<<"\t\t"<<"ay"<<"\t\t"<<"rx"<<"\t\t"<<"ry"<<"\t\t"<<"zx"<<"\t\t"<<"zy"<<"\t\t"<<"rvx"<<"\t\t"<<"rvy"<<endl;
    for(int i=0;i<N;i++){
        fout<<ax[i]<<"\tt"<<ay[i]<<"\t\t"<<rx[i]<<"\t\t"<<ry[i]<<"\t\t"<<zx[i]<<"\t\t"<<zy[i]<<"\t\t"<<rvx[i]<<"\t\t"<<rvy[i]<<endl;
    }
    */

    fout.close();

    return 0;
};

3.4 config.yaml部分

T: 0.01  #时间间隔
N: 1000  #数据量

stateSize: 4  #状态量x个数
measSize: 2  #观测量z个数
uSize: 2  #控制量u个数

out_path: "../data/data.txt"
u_path: ""
z_path: ""

A: [1, 0, 0.01, 0,
    0, 1, 0, 0.01,
    0, 0, 1, 0,
    0, 0, 0, 1]  #状态矩阵
B: [0.00005, 0,
    0, 0.00005,
    0.01, 0,
    0, 0.01]  #控制矩阵
H: [1, 0, 0, 0,
    0, 1, 0, 0]  #观测的矩阵
Q: [0.25, 0, 0, 0,
    0, 0.25, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1]  #控制协方差矩阵
R: [1, 0,
    0, 1] #观测协方差矩阵
x: [0, 0, 0, 0] #状态量初始值
P: [1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1]  #状态量初始值协方差矩阵

四、结果

后面应该是速度累积太大了，导致即使时间间隔设置的很小，也会产生极大的漂移

将数据量设置成500

nice！

某不知名GNSS研究僧
关注
从代码入手理解卡尔曼滤波器的原理之使用Eigen实现二维卡尔曼滤波器（七）
小秋 AI SLAM 入门实战教程
10-18 245
当然可以。现在，给定了矩阵和向量的具体定义，我们可以更详细地解释这些变量的意义，并确定卡尔曼滤波器的维度。F这意味着系统的状态从时刻t到t+1没有实质变化，即每个状态都保持不变。H这意味着观测值直接等于状态值，没有任何转换。Q这意味着两个状态的不确定性都是0.01，并且它们之间没有相关性。R这表明观测值有较大的不确定性，并且两个观测值之间没有相关性。x0这意味着滤波器的初始状态为零。P0这表示对初始状态的不确定性相对较大，并且两个状态之间没有相关性。综上所述，这是一个。
激光雷达数据作为观测讲解卡尔曼滤波器
小秋 AI SLAM 入门实战教程
10-24 744
可以通过传感器获取障碍物的位置、速度；但是任何测量结果都是有误差的。因此需要在传感器测量结果的基础上，进行跟踪，以此来保证障碍物的位置、速度等信息不会发生突变。所谓的跟踪就是通过运动模型来递推障碍物的位置、速度等信息。最经典的跟踪算法是卡尔曼滤波器。可应用于障碍物跟踪和多传感器融合定位用障碍物跟踪为例子介绍卡尔曼滤波器。卡尔曼滤波器就是根据上一时刻的状态，预测当前时刻的状态，将预测的状态与当前时刻的测量值进行加权，加权后的结果才认为是当前的实际状态，而不是仅仅听信当前的测量值。
Kalman滤波c++实现
01-30
本代码是卡尔曼滤波的c++实现方法，仅供参考。通过以下方式进行编译，执行。 cd kalman-cpp mkdir build cd build cmake .. make ./kalman-test
Kalman滤波器的C++实现
热门推荐
Alexon Xu的博客
08-16 1万+
1、原理介绍 kalman滤波在状态估计和多传感器融合等方面应用广泛，典型的应用有无人机的飞行姿态估计，汽车的行驶状态估计，无人驾驶中的激光雷达和其他传感器之间的融合等。至于Kalman滤波的数学推导，比较复杂，需要数学功底比较好，我看了很多次书还是不会推导，放弃了，拿过来用就行了。有点儿拿来主义了。还是的介绍下kalman滤波所涉及的系统微分方程和几个核心公式。kalman滤波分为预测和更新两...
C++实现卡尔曼滤波
To be a better man
10-03 3277
【代码】C++实现卡尔曼滤波。
Kalman滤波的C++实现
qq_36800378的博客
06-29 1485
Kalman类的头文件，调用处包含此头文件，然后设置初始值、获取数据、显示误差 #include <vector> #ifndef KALMAN_H #define KALMAN_H #define N 50 using namespace::std; class Kalman { public: Kalman(); /** * @brief setI...
小秋SLAM入门实战SLAM所有文章汇总
最新发布
小秋 AI SLAM 入门实战教程
12-29 1600
SLAM算法中的缓冲区和队列用途和用法 【ORB_SLAM2源码解读】Intel RealSense D435i 实时跑ORB_SLAM2单目 双目 RGBD 【从零开始学习SLAM】两帧之间的特征点视觉里程计并用g2o优化 【从零开始学习SLAM】VIZ Pangolin 可视化 【从零开始学习SLAM】BA问题用g2o求解重新投影误差优化相机位姿和路标点 「零基础从零开始写VO视觉里程计」匹配的3D空间点求位姿并用g2o优化 【ORB_SLAM2源码解读】Intel RealSense Depth Ca
SLAM算法大全，包括代码和文档、工具
01-16
概率机器人基于卡尔曼滤波器实现实时定位和地图创建（SLAM）算法 机器人地图创建新算法，DP-SLAM源程序 利用Matlab编写的基于EKF的SLAM仿真器源码 机器人定位中的EKF-SLAM算法，实现同时定位和地图构建 基于直线特征...
卡尔曼滤波C++代码实现.zip
06-29
用C++代码实现卡尔曼滤波，可直接运行，也可以单独调用类，设置参数然后获取数据
卡尔曼滤波KF算法C++实现
11-06
卡尔曼滤波算法类的C++实现，已经验证正确性，常加速度模型，使用了Eigen的矩阵运算，资源包含了KF类和Eigen库
卡尔曼滤波器与C++实现(kf, ekf)
Samuel_JO的博客
07-11 1286
​以简单例子介绍卡尔曼滤波器，并附有C++代码的卡尔曼实现
卡尔曼(Kalman)滤波 C++
weixin_38799245的博客
12-05 1326
关于卡尔曼滤波介绍请看：卡尔曼滤波_随风张幔的博客-CSDN博客_卡尔曼滤波 例如有这样的一个场景：无人车正常前行时前方突然出现一个行人，无人车就需要对前方行人的行为动作做一个预估和判断，以采取合适的避障或停车策略。如果想要估计一个行人的运动状态，首先需要建立被估计对象的状态方程表达式。人的状态可以用数学方程表示为，其中为行人的当前位置，而则是行人当前的速度。用一个向量来表示一个状态： 上述方程表示了行人的方向的位置
卡尔曼滤波(利用C++编写，ROS下实现)
er_dan_love的博客
05-27 6956
卡尔曼滤波 C++
卡尔曼滤波的C++实现
weixin_41856133的博客
07-18 1643
作者：博客如 来源：CSDN 原文：https://blog.csdn.net/m0_38089090/article/details/79523784 版权声明：本文为博主原创文章，转载请附上博文链接！ 转载只为日后好用,原创在上方. 写博客记录下做过的工作，毕竟好记性不如烂笔头。 卡尔曼滤波算法的博客很多，白巧克力亦唯心http://blog.csdn....
卡尔曼滤波原理及c++实现
ziyu2020的博客
12-22 1686
卡尔曼滤波是一种最优估计算法。用处：1）利用可测量值估算无法测量的量。2）对有测量噪声的物理量进行估计。常用于制导和导航、计算机视觉等。状态估计器[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Wyl6RMng-1671698056041)(C:\Users\admin\AppData\Roaming\Typora\typora-user-images\image-20221222145125380.png)]
移动机器人SLAM方法：扩展卡尔曼滤波器应用
"基于扩展卡尔曼滤波器的移动机器人SLAM研究" 移动机器人Simultaneous Localization and Mapping（SLAM）是机器人学中的一个核心问题，指的是机器人在未知环境中同时进行自我定位和环境建图的过程。本文针对这一...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值