卡尔曼滤波C++代码
#include <ros/ros.h>
#include <string>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <limits> using namespace std;
using Eigen::MatrixXd; //
double generateGaussianNoise(double mu, double sigma)
{
const double epsilon = std::numeric_limits<double>::min();
const double two_pi = 2.0*3.14159265358979323846; static double z0, z1;
static bool generate;
generate = !generate; if (!generate)
return z1 * sigma + mu; double u1, u2;
do
{
u1 = rand() * (1.0 / RAND_MAX);
u2 = rand() * (1.0 / RAND_MAX);
} while (u1 <= epsilon); z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
return z0 * sigma + mu;
} int main(int argc, char **argv)
{
std::cout<<"test qusetion2 start !"<<std::endl;
ros::init(argc,argv,"question2_node");
ros::NodeHandle node; ofstream fout("/media/kuang/code_test/result.txt"); double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数 const double delta_t = 0.1;//控制周期,100ms
const int num = ;//迭代次数
const double acc = ;//加速度,ft/m MatrixXd A(, );
A(, ) = ;
A(, ) = ;
A(, ) = delta_t;
A(, ) = ; MatrixXd B(, );
B(, ) = pow(delta_t, ) / ;
B(, ) = delta_t; MatrixXd H(, );//测量的是小车的位移,速度为0
H(, ) = ;
H(, ) = ; MatrixXd Q(, );//过程激励噪声协方差,假设系统的噪声向量只存在速度分量上,且速度噪声的方差是一个常量0.01,位移分量上的系统噪声为0
Q(, ) = ;
Q(, ) = ;
Q(, ) = ;
Q(, ) = 0.01; MatrixXd R(, );//观测噪声协方差,测量值只有位移,它的协方差矩阵大小是1*1,就是测量噪声的方差本身。
R(, ) = ; //time初始化,产生时间序列
vector<double> time(, );
for (decltype(time.size()) i = ; i != num; ++i) {
time[i] = i * delta_t;
//cout<<time[i]<<endl;
} MatrixXd X_real(, );
vector<MatrixXd> x_real, rand;
//生成高斯分布的随机数
for (int i = ; i<; ++i) {
MatrixXd a(, );
a(, ) = generateGaussianNoise(, sqrt());
rand.push_back(a);
}
//生成真实的位移值
for (int i = ; i < num; ++i) {
X_real(, ) = 0.5 * acc * pow(time[i], );
X_real(, ) = ;
x_real.push_back(X_real);
} //变量定义,包括状态预测值,状态估计值,测量值,预测状态与真实状态的协方差矩阵,估计状态和真实状态的协方差矩阵,初始值均为零
MatrixXd X_evlt = MatrixXd::Constant(, , ), X_pdct = MatrixXd::Constant(, , ), Z_meas = MatrixXd::Constant(, , ),
Pk = MatrixXd::Constant(, , ), Pk_p = MatrixXd::Constant(, , ), K = MatrixXd::Constant(, , );
vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;
x_evlt.push_back(X_evlt);
x_pdct.push_back(X_pdct);
z_meas.push_back(Z_meas);
pk.push_back(Pk);
pk_p.push_back(Pk_p);
k.push_back(K); //开始迭代
for (int i = ; i < num; ++i) {
//预测值
X_pdct = A * x_evlt[i - ] + B * acc;
x_pdct.push_back(X_pdct);
//预测状态与真实状态的协方差矩阵,Pk'
Pk_p = A * pk[i - ] * A.transpose() + Q;
pk_p.push_back(Pk_p);
//K:2x1
MatrixXd tmp(, );
tmp = H * pk_p[i] * H.transpose() + R;
K = pk_p[i] * H.transpose() * tmp.inverse();
k.push_back(K);
//测量值z
Z_meas = H * x_real[i] + rand[i];
z_meas.push_back(Z_meas);
//估计值
X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
x_evlt.push_back(X_evlt);
//估计状态和真实状态的协方差矩阵,Pk
Pk = (MatrixXd::Identity(, ) - k[i] * H) * pk_p[i];
pk.push_back(Pk);
} cout << "含噪声测量" << " " << "后验估计" << " " << "真值" << " " << endl;
for (int i = ; i < num; ++i) {
cout<<z_meas[i]<<" "<<x_evlt[i](,)<<" "<<x_real[i](,)<<endl;
fout << z_meas[i] << " " << x_evlt[i](, ) << " " << x_real[i](, ) << endl;
} fout.close();
getchar();
return ;
} 下面是python画图代码:
#! /usr/bin/env python import os
import math
import matplotlib.pyplot as plt
import numpy as np def mean(data):
sum=0
for i in data:
sum = sum+i
return sum/len(data) if __name__ == "__main__": file1 = "/media/kuang/code_test/result.txt" frame=[]
measure_noise = []
post_eval = []
groundtruth = []
counter = 0 # Read all data
document1 = open(file1,'rw+')
for line1 in document1:
split_line1 = line1.split()
counter = counter + 1
frame.append(counter)
measure_noise.append(float(split_line1[0]))
post_eval.append(float(split_line1[1]))
groundtruth.append(float(split_line1[2]))
document1.close() START = 0
END = len(frame)-1 #
frame_sub = frame[START:END]
measure_noise_sub = measure_noise[START:END]
post_eval_sub = post_eval[START:END]
groundtruth_sub = groundtruth[START:END] fig, ax1 = plt.subplots() color = 'blue'
ax1.set_xlabel('Frame No.')
ax1.set_ylabel('measure noise', color=color)
ax1.set_ylim(-10,500)
ax1.plot(frame_sub, measure_noise_sub,'s-', color=color)
ax1.tick_params(axis='y', labelcolor=color) ax2 = ax1.twinx() # instantiate a second axes that shares the same x-axis
color = 'red'
ax2.set_ylabel('post eval', labelpad=40,color=color) # we already handled the x-label with ax1
ax2.set_ylim(-10,500)
ax2.plot(frame_sub, post_eval_sub, 's-',color=color)
ax2.tick_params(axis='y',pad=30.0, labelcolor=color) ax3 = ax1.twinx()
color = 'yellow'
ax3.set_ylabel('groundtruth', color=color, labelpad=30)
ax3.set_ylim(-10,500)
ax3.tick_params(axis='y', labelcolor=color)
ax3.plot(frame_sub, groundtruth_sub, 's-' , color=color) fig.tight_layout() # otherwise the right y-label is slightly clipped
ax1.grid()
plt.title("Kalman Test")
plt.show() # SAVE RESULT TO FILE
卡尔曼滤波C++代码的更多相关文章
- 卡尔曼滤波学习笔记1-Matlab模拟温度例子--代码比较乱,还需优化
温度模拟参数选取 xk 系统状态 实际温度 A 系统矩阵 温度不变,为1 B.uk 状态的控制量 无控制量,为0 Zk 观测值 温度计读数 H 观测矩阵 直接读出,为1 wk 过程噪声 温度变化偏差, ...
- 扩展卡尔曼滤波(MRPT)
EKF relies on a linearisation of the evolution and observation functions which are good approximatio ...
- 卡尔曼滤波(Kalman Filter)
一.引言 以下我们引用文献[1]中的一段话作为本文的開始: 想象你在黄昏时分看着一仅仅小鸟飞行穿过浓密的丛林.你仅仅能隐隐约约.断断续续地瞥见小鸟运动的闪现.你试图努力地猜測小鸟在哪里以及下一时刻它会 ...
- Google Cardboard的九轴融合算法——基于李群的扩展卡尔曼滤波
Google Cardboard的九轴融合算法 --基于李群的扩展卡尔曼滤波 极品巧克力 前言 九轴融合算法是指通过融合IMU中的加速度计(三轴).陀螺仪(三轴).磁场计(三轴),来获取物体姿态的方法 ...
- 树莓派 连接 JY901(MPU9250) python 代码
先说BUG,最近要做项目需要树莓派和陀螺仪,资金充足的话肯定是买一个硬件卡尔曼滤波的传感器类似JY901模块,资金不足的就买MPU6050. 网上关于MPU6050在树莓派上的代码还能用,关于JY90 ...
- 从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码
首发于公众号:计算机视觉life 旗下知识星球「从零开始学习SLAM」 这可能是最清晰讲解g2o代码框架的文章 理解图优化,一步步带你看懂g2o框架 小白:师兄师兄,最近我在看SLAM的优化算法,有种 ...
- 卡尔曼滤波(Kalman Filter) ZZ
一.引言 以下我们引用文献[1]中的一段话作为本文的開始: 想象你在黄昏时分看着一仅仅小鸟飞行穿过浓密的丛林.你仅仅能隐隐约约.断断续续地瞥见小鸟运动的闪现.你试图努力地猜測小鸟在哪里以及下一时刻它会 ...
- 卡尔曼滤波(Kalman Filter)在目标边框预测中的应用
1.卡尔曼滤波的导论 卡尔曼滤波器(Kalman Filter),是由匈牙利数学家Rudolf Emil Kalman发明,并以其名字命名.卡尔曼出生于1930年匈牙利首都布达佩斯.1953,1954 ...
- 滤波器算法(1)-卡尔曼滤波小车附带题目与MATLAB程序
1 简介 由卡尔曼这个学者提出的最佳线性滤波器,单纯时域维度即可实现[无需进行频域变换] 2 思路 由上一时刻的最佳估计值XKE_P预测①当前时刻预测值Pxv 与 ②当前时刻的测量值Mxv 进行联立计 ...
随机推荐
- 【OCR系列之一】字符识别技术总览
最近入坑研究OCR,看了比较多关于OCR的资料,对OCR的前世今生也有了一个比较清晰的了解.所以想写一篇关于OCR技术的综述,对OCR相关的知识点都好好总结一遍,以加深个人理解. 什么是OCR? OC ...
- app——升级测试点
APP版本升级的测试点 该文章转载于:https://www.cnblogs.com/changpuyi/p/8618755.html 移动端版本更新升级是一个比较重要的功能点,主要分为强制更新和 ...
- Nginx之前后端分离(入门)
几个月前,公司架构优化,首先就是前后端分离. 所谓前后端分离,就是在传统的前后端代码都在一个项目里的基础上,将前后端代码抽离,把前端代码从后端项目了分离出来,前后端开发人员各自在自己的项目里开发. 为 ...
- 使用VS2017+WDK10开发xp驱动
VS2017+WDK10使用默认配置可以开发出运行在win7的驱动,但要开发运行在xp的驱动,还要进行如下操作: Dervier Settings -> Driver Model -> t ...
- Java开发桌面程序学习(五)——文件选择器和目录选择器的使用
选择器的使用 DirectoryChooser目录选择器官方文档 FileChooser文件选择器官方文档 文件选择器的使用 JavaFx中有个FileChoser,可以打开一个对话框来选择文件 Fi ...
- WPF后台代码实现TextBlock滚动条
方法一: 常规的WPF操作: <ScrollViewer Width=" VerticalScrollBarVisibility="Auto" Horizontal ...
- C#用Call代替CallVirt之后的测试用例
一. C# 原始代码和直接结果 测试 C# 代码: class Program { static void Main(string[] args) { A c1 = new C(); c1.Foo() ...
- struts图片上传
文件上传:三种上传方案1.上传到tomcat服务器 上传图片的存放位置与tomcat服务器的耦合度太高2.上传到指定文件目录,添加服务器与真实目录的映射关系,从而解耦上传文件与tomcat的关系文件服 ...
- js获取手机唯一标识码
Device模块管理设备信息,用于获取手机设备的相关信息,如IMEI.IMSI.型号.厂商等.通过plus.device获取设备信息管理对象. imei: 设备的国际移动设备身份码 imsi: 设备的 ...
- 0基础入门学习Python(第4章)
第四章,了不起的分支和循环 4.1 分支和循环 Python主要依靠缩进来区分代码块 4.2 快速上手 成绩按照分数来划分等级,90分以上为A,80~90 为B,60~80 为C,60以下为D p4_ ...