一维二维卡尔曼滤波C++ ROS节点

订阅高斯噪声,作为观测值(二维的就是将噪声添加到匀加速的里程上形成带噪声的观测),卡尔曼滤波处理,处理结果发布出去,用matplot显示.

高斯噪声:

import random
import numpy
import rospy
from  std_msgs.msg import Float32

class pub_gauss():
    def __init__(self,means,sigma):
        rospy.init_node('pub_gauss', anonymous=False)
        pub = rospy.Publisher('gauss_noise', Float32, queue_size=10)
        rate = rospy.Rate(10)  # 10hz

        while not rospy.is_shutdown():

            noise = random.gauss(means, sigma)
            rospy.loginfo(noise)
            pub.publish(noise)
            rate.sleep()

if __name__ == '__main__':
    try:
        pub_gauss(0,0.8)
    except rospy.ROSInterruptException:
        pass

一维:

#include <iostream>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
using namespace std;
class kalman_filter
 {
public:
      kalman_filter(float q,float r)
    {
        Q=q;
        R=r;//如