精华内容
下载资源
问答
  • 试利用扩展卡尔曼滤波理论求出的最优估计。 要求: (1)利用Matlab或Python 编写仿真程序。 (2)给出各状态变量的真值和估计值曲线变化图。 (3)分别给出的真值与估计值之间的误差曲线变化图,并求出误差的均值和...
  • 试利用扩展卡尔曼滤波理论求出的最优估计。 要求: (1)利用Matlab或Python 编写仿真程序。 (2)在同一张图中,给出的真值和估计值曲线。 (3)给出的真值与估计值之间的误差曲线变化图,并求出误差的均值和方差...
  • t**2 / 2], [delta_t]]) # 状态矩阵 x = x_initial # 误差协方差矩阵 P = P_initial # 记录卡尔曼滤波器计算得到的距离 x_result = [] # 记录卡尔曼滤波器的时间 time_result = [] # 记录卡尔曼滤波器得到的速度 v_...
    # 生成汽车行驶的真实数据
    
    # 汽车从以初速度v0,加速度a行驶10秒钟,然后匀速行驶20秒
    # x0:initial distance, m
    # v0:initial velocity, m/s
    # a:acceleration,m/s^2
    # t1:加速行驶时间,s
    # t2:匀速行驶时间,s
    # dt:interval time, s
    
    import numbers
    import numpy as np
    import matplotlib.pyplot as plt
    import math
    
    def generate_data(x0, v0, a, t1, t2, dt):
        a_current = a
        v_current = v0
        t_current = 0
    
        # 记录汽车运行的真实状态
        a_list = []
        v_list = []
        t_list = []
    
        # 汽车运行的两个阶段
        # 第一阶段:加速行驶
        while t_current <= t1:
            # 记录汽车运行的真实状态
            a_list.append(a_current)
            v_list.append(v_current)
            t_list.append(t_current)
            # 汽车行驶的运动模型
            v_current += a * dt
            t_current += dt
    
        # 第二阶段:匀速行驶
        a_current = 0
        while t2 > t_current >= t1:
            # 记录汽车运行的真实状态
            a_list.append(a_current)
            v_list.append(v_current)
            t_list.append(t_current)
            # 汽车行驶的运动模型
            t_current += dt
    
        # 计算汽车行驶的真实距离
        x = x0
        x_list = [x0]
        for i in range(len(t_list) - 1):
            tdelta = t_list[i+1] - t_list[i]
            x = x + v_list[i] * tdelta + 0.5 * a_list[i] * tdelta**2
            x_list.append(x)
        return t_list, x_list, v_list, a_list
    
    # 生成雷达获得的数据。需要考虑误差,误差呈现高斯分布
    def generate_lidar(x_list, standard_deviation):
        return x_list + np.random.normal(0, standard_deviation, len(x_list))
    
    # 获取汽车行驶的真实状态
    t_list, x_list, v_list, a_list = generate_data(100, 5, 4, 10, 20, 0.1)
    
    # 创建激光雷达的测量数据
    # 测量误差的标准差。为了方便观测,可以增加该值。
    standard_deviation = 0.15
    # 雷达测量得到的距离
    lidar_x_list = generate_lidar(x_list, standard_deviation)
    # 雷达测量的时间
    lidar_t_list = t_list
    
    # 可视化.创建包含2*3个子图的视图
    fig, ((ax1, ax2, ax3), (ax4, ax5, ax6)) = plt.subplots(2, 3, figsize=(20, 15))
    
    # 真实距离
    ax1.set_title("truth distance")
    ax1.set_xlabel("time")
    ax1.set_ylabel("distance")
    ax1.set_xlim([0, 21])
    ax1.set_ylim([0, 1000])
    ax1.plot(t_list, x_list)
    
    # 真实速度
    ax2.set_title("truth velocity")
    ax2.set_xlabel("time")
    ax2.set_ylabel("velocity")
    ax2.set_xlim([0, 21])
    ax2.set_ylim([0, 50])
    ax2.set_xticks(range(21))
    ax2.set_yticks(range(0, 50, 5))
    ax2.plot(t_list, v_list)
    
    # 真实加速度
    ax3.set_title("truth acceleration")
    ax3.set_xlabel("time")
    ax3.set_ylabel("acceleration")
    ax3.set_xlim([0, 21])
    ax3.set_ylim([0, 5])
    ax3.plot(t_list, a_list)
    
    # 激光雷达测量结果
    ax4.set_title("Lidar measurements VS truth")
    ax4.set_xlabel("time")
    ax4.set_ylabel("distance")
    ax4.set_xlim([0, 21])
    ax4.set_ylim([0, 1000])
    ax4.set_xticks(range(21))
    ax4.set_yticks(range(0, 1000, 100))
    ax4.plot(t_list, x_list, label="truth distance")
    ax4.scatter(lidar_t_list, lidar_x_list, label="Lidar distance", color="red", marker="o", s=2)
    ax4.legend()
    # plt.show()##观查数据
    
    
    # 使用卡尔曼滤波器
    # 初始距离。注意:这里假设初始距离为0,因为无法测量初始距离。
    initial_distance = 0
    
    # 初始速度。注意:这里假设初始速度为0,因为无法测量初始速度。
    initial_velocity = 0
    
    # 卡尔曼滤波器需要调用的矩阵类
    class Matrix(object):
        # 构造矩阵
        def __init__(self, grid):
            self.g = np.array(grid)
            self.h = len(grid)
            self.w = len(grid[0])
    
        # 单位矩阵
        @staticmethod
        def identity( n):
            return Matrix(np.eye(n))
    
        # 矩阵的迹
        def trace(self):
            if not self.is_square():
                raise(ValueError, "Cannot calculate the trace of a non-square matrix.")
            else:
                return self.g.trace()
        # 逆矩阵
        def inverse(self):
            if not self.is_square():
                raise(ValueError, "Non-square Matrix does not have an inverse.")
            if self.h > 2:
                raise(NotImplementedError, "inversion not implemented for matrices larger than 2x2.")
            if self.h == 1:
                m = Matrix([[1/self[0][0]]])
                return m
            if self.h == 2:
                try:
                    m = Matrix(np.matrix(self.g).I)
                    return m
                except np.linalg.linalg.LinAlgError as e:
                    print("Determinant shouldn't be zero.", e)
    
        # 转置矩阵
        def T(self):
            T = self.g.T
            return Matrix(T)
    
        # 判断矩阵是否为方阵
        def is_square(self):
            return self.h == self.w
    
        # 通过[]访问
        def __getitem__(self,idx):
            return self.g[idx]
    
        # 打印矩阵的元素
        def __repr__(self):
            s = ""
            for row in self.g:
                s += " ".join(["{} ".format(x) for x in row])
                s += "\n"
            return s
    
        # 加法
        def __add__(self,other):
            if self.h != other.h or self.w != other.w:
                raise(ValueError, "Matrices can only be added if the dimensions are the same")
            else:
                return Matrix(self.g + other.g)
    
        # 相反数
        def __neg__(self):
            return Matrix(-self.g)
    
        #减法
        def __sub__(self, other):
            if self.h != other.h or self.w != other.w:
                raise(ValueError, "Matrices can only be subtracted if the dimensions are the same")
            else:
                return Matrix(self.g - other.g)
    
        # 矩阵乘法:两个矩阵相乘
        def __mul__(self, other):
            if self.w != other.h:
                raise(ValueError, "number of columns of the pre-matrix must equal the number of rows of the post-matrix")
            return Matrix(np.dot(self.g, other.g))
    
        # 标量乘法:变量乘以矩阵
        def __rmul__(self, other):
            if isinstance(other, numbers.Number):
                return Matrix(other * self.g)
    
    # 状态矩阵的初始值
    x_initial = Matrix([[initial_distance], [initial_velocity]])
    
    # 误差协方差矩阵的初始值
    P_initial = Matrix([[5, 0], [0, 5]])
    
    # 加速度方差
    acceleration_variance = 50
    
    # 雷达测量结果方差
    lidar_variance = standard_deviation**2
    
    # 观测矩阵,联系预测向量和测量向量
    H = Matrix([[1, 0]])
    
    # 测量噪音协方差矩阵。因为测量值只有位置一个变量,所以这里是位置的方差。
    R = Matrix([[lidar_variance]])
    
    # 单位矩阵I = Matrix.indentity(2)报错
    
    I = Matrix([[1, 0]])
    I=I.identity(2)
    print(I)
    
    # 状态转移矩阵
    def F_matrix(delta_t):
        return Matrix([[1, delta_t], [0, 1]])
    
    # 外部噪音协方差矩阵
    def Q_matrix(delta_t, variance):
        t4 = math.pow(delta_t, 4)
        t3 = math.pow(delta_t, 3)
        t2 = math.pow(delta_t, 2)
        return variance * Matrix([[(1/4)*t4, (1/2)*t3], [(1/2)*t3, t2]])
    
    def B_matrix(delta_t):
        return Matrix([[delta_t**2 / 2], [delta_t]])
    
    # 状态矩阵
    x = x_initial
    
    # 误差协方差矩阵
    P = P_initial
    
    # 记录卡尔曼滤波器计算得到的距离
    x_result = []
    
    # 记录卡尔曼滤波器的时间
    time_result = []
    
    # 记录卡尔曼滤波器得到的速度
    v_result = []
    
    for i in range(len(lidar_x_list) - 1):
        delta_t = (lidar_t_list[i + 1] - lidar_t_list[i])
        # 预测
        F = F_matrix(delta_t)
        Q = Q_matrix(delta_t, acceleration_variance)
    
        # 注意:运动模型使用的是匀速运动,汽车实际上有一段时间是加速运动的
        x_prime = F * x
        P_prime = F * P * F.T() + Q
    
        # 更新
        # 测量向量和状态向量的差值。注意:第一个时刻是没有测量值的,
        # 只有经过一个脉冲周期,才能获得测量值。
        y = Matrix([[lidar_x_list[i + 1]]]) - H * x_prime
        S = H * P_prime * H.T() + R
        K = P_prime * H.T() * S.inverse()
        x = x_prime + K * y
        P = (I - K * H) * P_prime
        x_result.append(x[0][0])
        v_result.append(x[1][0])
        time_result.append(lidar_t_list[i+1])
    
        # 把真实距离、激光雷达测量的距离以及卡尔曼滤波器的结果(距离)可视化
    ax5.set_title("Lidar measurements VS truth")
    ax5.set_xlabel("time")
    ax5.set_ylabel("distance")
    ax5.set_xlim([0, 21])
    ax5.set_ylim([0, 1000])
    ax5.set_xticks(range(0, 21, 2))
    ax5.set_yticks(range(0, 1000, 100))
    ax5.plot(t_list, x_list, label="truth distance", color="blue", linewidth=1)
    ax5.scatter(lidar_t_list, lidar_x_list, label="Lidar distance", color="red", marker="o", s=2)
    ax5.scatter(time_result, x_result, label="kalman", color="green", marker="o", s=2)
    ax5.legend()
    
    # 把真实速度、卡尔曼滤波器的结果(速度)可视化
    ax6.set_title("Lidar measurements VS truth")
    ax6.set_xlabel("time")
    ax6.set_ylabel("velocity")
    ax6.set_xlim([0, 21])
    ax6.set_ylim([0, 50])
    ax6.set_xticks(range(0, 21, 2))
    ax6.set_yticks(range(0, 50, 5))
    ax6.plot(t_list, v_list, label="truth velocity", color="blue", linewidth=1)
    ax6.scatter(time_result, v_result, label="Lidar velocity", color="red", marker="o", s=2)
    ax6.legend()
    
    plt.show()
    

    在这里插入图片描述

    展开全文
  • 卡尔曼滤波的简单工程应用。

    卡尔曼滤波的简单工程应用。

    介绍

    扩展卡尔曼滤波(Extended kalman filter,EKF)一种非线性卡尔曼滤波,用来估计均值(mean)和协方差(covariance),广泛用于非线性机器人状态估计、GPS、导航。

    例子

    蓝色的是要求轨迹,绿色的是GPS定位轨迹,黑色的惯性导航轨迹,红色的是EKF轨迹。
    在这里插入图片描述

    编程思想

    利用EKF与要求轨迹和其他算法得出的轨迹对比。

    代码分析

    导入

    import math
    
    import matplotlib.pyplot as plt
    import numpy as np
    

    参数设置

    # Covariance for EKF simulation
    Q = np.diag([
        0.1,  # variance of location on x-axis
        0.1,  # variance of location on y-axis
        np.deg2rad(1.0),  # variance of yaw angle
        1.0  # variance of velocity
    ]) ** 2  # predict state covariance
    R = np.diag([1.0, 1.0]) ** 2  # Observation x,y position covariance
    
    #  Simulation parameter 
    # noise
    INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
    GPS_NOISE = np.diag([0.5, 0.5]) ** 2
    
    DT = 0.1  # time tick [s]
    SIM_TIME = 50.0  # simulation time [s]
    
    show_animation = True
    

    main()

    def main():
        print(__file__ + " start!!")
    
        time = 0.0
    
        # State Vector [x y yaw v]'
        xEst = np.zeros((4, 1))
        xTrue = np.zeros((4, 1))
        PEst = np.eye(4)
    
        xDR = np.zeros((4, 1))  # Dead reckoning
    
        # history
        hxEst = xEst
        hxTrue = xTrue
        hxDR = xTrue
        hz = np.zeros((2, 1))
    
        while SIM_TIME >= time:
            time += DT
            u = calc_input()
    
            xTrue, z, xDR, ud = observation(xTrue, xDR, u)
    
            xEst, PEst = ekf_estimation(xEst, PEst, z, ud)
    
            # store data history
            hxEst = np.hstack((hxEst, xEst))
            hxDR = np.hstack((hxDR, xDR))
            hxTrue = np.hstack((hxTrue, xTrue))
            hz = np.hstack((hz, z))
    
            if show_animation:
                plt.cla()
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect('key_release_event',
                        lambda event: [exit(0) if event.key == 'escape' else None])
                plt.plot(hz[0, :], hz[1, :], ".g")
                plt.plot(hxTrue[0, :].flatten(),
                         hxTrue[1, :].flatten(), "-b")
                plt.plot(hxDR[0, :].flatten(),
                         hxDR[1, :].flatten(), "-k")
                plt.plot(hxEst[0, :].flatten(),
                         hxEst[1, :].flatten(), "-r")
                plot_covariance_ellipse(xEst, PEst)
                plt.axis("equal")
                plt.grid(True)
                plt.pause(0.001)
    

    完整代码

    请关注AtsushiSakai的PythonRobotics

    展开全文
  • 基于Python实现扩展卡尔曼滤波算法

    千次阅读 2020-06-16 00:47:54
    2、实现扩展卡尔曼滤波器的预备步骤 (1)阶段1 新矩阵的构建 (2) 阶段2 空间模型线性化 3、扩展卡尔曼滤波器的实现 输入过程: [y1,y2,...,yn][y_1,y_2,...,y_n][y1​,y2​,...,yn​] 已知参数: 非线性状态向量...

    1、卡尔曼滤波器定义式的变形
    (1)新息过程
    a n = y n − b n ( x ^ n ∣ n − 1 ) a_n = y_n-b_n(\hat x_{n|n-1}) an=ynbn(x^nn1)
    (2)状态空间
    X n + 1 = A n + 1 , n X n + w n + ε n X_{n+1} = A_{n+1,n}X_n+w_n+ \varepsilon_n Xn+1=An+1,nXn+wn+εn
    y n = B n X n + v n y_n=B_nX_n+v_n yn=BnXn+vn
    (3)表述形式
    x ^ n + 1 ∣ n = A n + 1 , n x ^ n ∣ n + ε n \hat x_{n+1|n}=A_{n+1,n}\hat x_{n|n} + \varepsilon_n x^n+1n=An+1,nx^nn+εn
    2、实现扩展卡尔曼滤波器的预备步骤
    扩展卡尔曼滤波的基本思想是在没个时间点,围绕最近状态估计结果中的状态空间模型线性化
    (1)阶段1 新矩阵的构建
    A n + 1 , n = ∂ a n ( X ) ∂ X ∣ X = x ^ n ∣ n A_{n+1,n}=\frac{\partial a_n(X)}{\partial X}|_{X=\hat{x}_{n|n}} An+1,n=Xan(X)X=x^nn
    B n = ∂ b n ∂ x ∣ x = x n ∣ n − 1 ^ B_n=\frac{\partial b_n}{\partial x}|_{x=\hat{x_{n|n-1}}} Bn=xbnx=xnn1^
    (2) 阶段2 空间模型线性化
    3、扩展卡尔曼滤波器的实现

    输入过程:
    [ y 1 , y 2 , . . . , y n ] [y_1,y_2,...,y_n] [y1,y2,...,yn]
    已知参数:
    非线性状态向量函数= a n ( x n ) a_n(x_n) an(xn)
    非线性测量向量函数= b n ( x n ) b_n(x_n) bn(xn)
    过程噪声向量的协方差矩阵= Q w , n Q_{w,n} Qw,n
    测量噪声向量的协方差矩阵= Q v , n Q_{v,n} Qv,n
    计算:n=1,2,3…
    G n = P n , n − 1 B n T [ B n P n , n − 1 B n T + Q v , n ] G_n = P_{n,n-1} B_n ^T[B_nP_{n,n-1}B_n^T + Q_{v,n}] Gn=Pn,n1BnT[BnPn,n1BnT+Qv,n]
    a n = y n − b n ( x ^ n ∣ n − 1 ) a_n = y_n-b_n(\hat x_{n|n-1}) an=ynbn(x^nn1)
    x ^ n ∣ n = x ^ n ∣ n − 1 + G n a n \hat x_{n|n}=\hat x_{n|n-1} + G_n a_n x^nn=x^nn1+Gnan
    x ^ n + 1 ∣ n = a n ( x ^ n ∣ n ) \hat x_{n+1|n} = a_n(\hat x_{n|n}) x^n+1n=an(x^nn)
    P n ∣ n = P n ∣ n − 1 − G n B n P n ∣ n + 1 P_{n|n} = P_{n|n-1}-G_nB_nP_{n|n+1} Pnn=Pnn1GnBnPnn+1
    P n + 1 ∣ n = A n + 1 , n P n ∣ n A n + 1 , n T + Q w , n P_{n+1|n} = A_{n+1,n}P_{n|n}A_{n+1,n}^T+Q_{w,n} Pn+1n=An+1,nPnnAn+1,nT+Qw,n

    4、代码实现
    该代码来源于https://github.com/AtsushiSakai/PythonRobotics

    """
    
    Extended kalman filter (EKF) localization sample
    
    author: Atsushi Sakai (@Atsushi_twi)
    
    """
    
    import math
    
    import matplotlib.pyplot as plt
    import numpy as np
    from scipy.spatial.transform import Rotation as Rot
    
    # Covariance for EKF simulation
    Q = np.diag([
        0.1,  # variance of location on x-axis
        0.1,  # variance of location on y-axis
        np.deg2rad(1.0),  # variance of yaw angle
        1.0  # variance of velocity
    ]) ** 2  # predict state covariance
    R = np.diag([1.0, 1.0]) ** 2  # Observation x,y position covariance
    
    #  Simulation parameter
    INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
    GPS_NOISE = np.diag([0.5, 0.5]) ** 2
    
    DT = 0.1  # time tick [s]
    SIM_TIME = 50.0  # simulation time [s]
    
    show_animation = True
    
    
    def calc_input():
        v = 1.0  # [m/s]
        yawrate = 0.1  # [rad/s]
        u = np.array([[v], [yawrate]])
        return u
    
    
    def observation(xTrue, xd, u):
        xTrue = motion_model(xTrue, u)
    
        # add noise to gps x-y
        z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
    
        # add noise to input
        ud = u + INPUT_NOISE @ np.random.randn(2, 1)
    
        xd = motion_model(xd, ud)
    
        return xTrue, z, xd, ud
    
    
    def motion_model(x, u):
        F = np.array([[1.0, 0, 0, 0],
                      [0, 1.0, 0, 0],
                      [0, 0, 1.0, 0],
                      [0, 0, 0, 0]])
    
        B = np.array([[DT * math.cos(x[2, 0]), 0],
                      [DT * math.sin(x[2, 0]), 0],
                      [0.0, DT],
                      [1.0, 0.0]])
    
        x = F @ x + B @ u
    
        return x
    
    
    def observation_model(x):
        H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])
    
        z = H @ x
    
        return z
    
    
    def jacob_f(x, u):
        """
        Jacobian of Motion Model
    
        motion model
        x_{t+1} = x_t+v*dt*cos(yaw)
        y_{t+1} = y_t+v*dt*sin(yaw)
        yaw_{t+1} = yaw_t+omega*dt
        v_{t+1} = v{t}
        so
        dx/dyaw = -v*dt*sin(yaw)
        dx/dv = dt*cos(yaw)
        dy/dyaw = v*dt*cos(yaw)
        dy/dv = dt*sin(yaw)
        """
        yaw = x[2, 0]
        v = u[0, 0]
        jF = np.array([
            [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
            [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0]])
    
        return jF
    
    
    def jacob_h():
        # Jacobian of Observation Model
        jH = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])
    
        return jH
    
    
    def ekf_estimation(xEst, PEst, z, u):
        #  Predict
        xPred = motion_model(xEst, u)
        jF = jacob_f(xEst, u)
        PPred = jF @ PEst @ jF.T + Q
    
        #  Update
        jH = jacob_h()
        zPred = observation_model(xPred)
        y = z - zPred
        S = jH @ PPred @ jH.T + R
        K = PPred @ jH.T @ np.linalg.inv(S)
        xEst = xPred + K @ y
        PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
        return xEst, PEst
    
    
    def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
        Pxy = PEst[0:2, 0:2]
        eigval, eigvec = np.linalg.eig(Pxy)
    
        if eigval[0] >= eigval[1]:
            bigind = 0
            smallind = 1
        else:
            bigind = 1
            smallind = 0
    
        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
        a = math.sqrt(eigval[bigind])
        b = math.sqrt(eigval[smallind])
        x = [a * math.cos(it) for it in t]
        y = [b * math.sin(it) for it in t]
        angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
        rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
        fx = rot @ (np.array([x, y]))
        px = np.array(fx[0, :] + xEst[0, 0]).flatten()
        py = np.array(fx[1, :] + xEst[1, 0]).flatten()
        plt.plot(px, py, "--r")
    
    
    def main():
        print(__file__ + " start!!")
    
        time = 0.0
    
        # State Vector [x y yaw v]'
        xEst = np.zeros((4, 1))
        xTrue = np.zeros((4, 1))
        PEst = np.eye(4)
    
        xDR = np.zeros((4, 1))  # Dead reckoning
    
        # history
        hxEst = xEst
        hxTrue = xTrue
        hxDR = xTrue
        hz = np.zeros((2, 1))
    
        while SIM_TIME >= time:
            time += DT
            u = calc_input()
    
            xTrue, z, xDR, ud = observation(xTrue, xDR, u)
    
            xEst, PEst = ekf_estimation(xEst, PEst, z, ud)
    
            # store data history
            hxEst = np.hstack((hxEst, xEst))
            hxDR = np.hstack((hxDR, xDR))
            hxTrue = np.hstack((hxTrue, xTrue))
            hz = np.hstack((hz, z))
    
            if show_animation:
                plt.cla()
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect('key_release_event',
                        lambda event: [exit(0) if event.key == 'escape' else None])
                plt.plot(hz[0, :], hz[1, :], ".g")
                plt.plot(hxTrue[0, :].flatten(),
                         hxTrue[1, :].flatten(), "-b")
                plt.plot(hxDR[0, :].flatten(),
                         hxDR[1, :].flatten(), "-k")
                plt.plot(hxEst[0, :].flatten(),
                         hxEst[1, :].flatten(), "-r")
                plot_covariance_ellipse(xEst, PEst)
                plt.axis("equal")
                plt.grid(True)
                plt.pause(0.001)
    
    
    if __name__ == '__main__':
        main()
    
    
    展开全文
  • 使用MATLAB进行卡尔曼滤波(9轴)IMU数据 这是用于9轴IMU传感器的卡尔曼滤波算法。 (加速度计,陀螺仪,磁力计) 您可以看到带有数据的图形化IMU传感器。 测验 示范 -将很快添加。 特征 动画情节 时间线 硬铁偏置...
  • 卡尔曼滤波算法计算电池SOC,simulink模型,该模型用于参考
  • 卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果。 所以之前为了保证我们的处理模型是线性的,我们上一节中使用了恒定...

     

    前言:

    • 卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果。

    • 所以之前为了保证我们的处理模型是线性的,我们上一节中使用了恒定速度模型,然后将估计目标的加减速用处理噪声来表示,这一模型用来估算行人的状态其实已经足够了

    • 但是在现实的驾驶环境中,我们不仅要估计行人,我们除了估计行人状态以外,我们还需要估计其他车辆,自行车等等状态,他们的状态估计显然不能使用简单的线性系统来描述

    • 这里,我们介绍非线性系统情况下的一种广泛使用的滤波算法——扩展卡尔曼滤波(Extended Kalman Filter, EKF)

    • 本节讲解非线性系统中广泛使用的扩展卡尔曼滤波算法,我们通常将该算法应用于实际的车辆状态估计(或者说车辆追踪)中。

    • 另外,实际的车辆追踪运动模型显然不能使用简单的恒定速度模型来建模

    • 在本节中会介绍几种应用于车辆追踪的高级运动模型。

    • 并且已经其中的CTRV模型来构造我们的扩展卡尔曼滤波。

    • 最后,在代码实例中,我会介绍如何使用EKF做多传感器融合

    应用于车辆追踪的高级运动模型

    • 首先要明确的一点是,不管是什么运动模型,本质上都是为了帮助我们简化问题,所以我们可以根据运动模型的复杂程度(次数)来给我们常用的运动模型分一下类。

    1. 一次运动模型(也别称为线性运动模型):

    • 恒定速度模型(Constant Velocity, CV)

    • 恒定加速度模型(Constant Acceleration, CA)

    • 这些线性运动模型假定目标是直线运动的,并不考虑物体的转弯。

    2. 二次运动模型:

    • 恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)

    • 恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

    3. CTRV

    • CTRV目前多用于机载追踪系统(飞机),这些二次运动模型大多假定速度 vv 和 偏航角速度(yaw rate) ωω 没有关系,因此,在这类运动模型中,由于偏航角速度测量的扰动(不稳定),即使车辆没有移动,我们的运动模型下的角速度也会发生细微的变化。

    • 为了解决这个问题,速度 vv 和 偏航角速度 ωω 的关联可以通过设定转向角 ΦΦ 恒定来建立,这样就引出了 恒定转向角和速度模型(Constant Steering Angle and Velocity,CSAV), 另外,速度可以别假定为线性变化的,进而引出了常曲率和加速度模型(Constant Curvature and Acceleration,CCA)。

    这些运动模型的关系如图:

    运动模型的状态转移公式

    • 由于除CCA以外,以上的运动模型都非常著名,故本文不提供详细的推导过程。本文提供CV和CTRV模型的状态转移公式。

    • 状态转移公式:就是我们的处理模型由上一状态的估计计算下一个状态的先验分布的计算公式,可以理解为我们基于一定的先验

    • 知识总结出来的运动公式。

     

    • 扩展卡尔曼滤波

    1. 雅可比矩阵

    1. 处理噪声

     

     

     

    • 扩展卡尔曼滤波

    1. 雅可比矩阵

    1. 处理噪声

    将预测映射到激光雷达测量空间:

    虽然这个雅可比矩阵看似非常复杂,但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式,在本篇中,我们采用numdifftools这个公式来直接求解雅可比矩阵。

    综上,EKF的整个过程为:

    4.Python实现

    • 和之前一样,为了实现交互式的代码,实例的代码为了便于理解,我们仍然使用大家熟悉的Python来实现

    • 当然,实际无人车项目肯定是需要用C++来实现的,要将下面的示例代码使用C++来改写是非常简单快速的。

    • 首先引入相关的库:

    from __future__ import print_function
    
    import numpy as np
    
    import matplotlib.dates as mdates
    
    import matplotlib.pyplot as plt
    
    from scipy.stats import norm
    
    from sympy import Symbol, symbols, Matrix, sin, cos, sqrt, atan2
    
    from sympy import init_printing
    
    init_printing(use_latex=True)
    
    import numdifftools as nd
    
    import math
    • 首先我们读取我们的数据集,该数据集包含了追踪目标的LIDAR和RADAR的测量值,以及测量的时间点

    • 同时为了验证我们追踪目标的精度,该数据还包含了追踪目标的真实坐标。(数据下载链接见文章末尾)

     

    • 其中第一列表示测量数据来自LIDAR还是RADAR,LIDAR的2,3列表示测量的目标 (x,y)(x,y),第4列表示测量的时间点,第5,6,7,8表示真实的(x,y,vx,vy)(x,y,vx,vy), RADAR测量的(前三列)是(ρ,ψ,ρ˙)(ρ,ψ,ρ˙),其余列的数据的意义和LIDAR一样。

    • 首先我们读取整个数据:

    dataset = []

     

    # read the measurement data, use 0.0 to stand LIDAR data

    # and 1.0 stand RADAR data

    with open('data_synthetic.txt', 'rb') as f:

    lines = f.readlines()

    for line in lines:

    line = line.strip('\n')

    line = line.strip()

    numbers = line.split()

    result = []

    for i, item in enumerate(numbers):

    item.strip()

    if i == 0:

    if item == 'L':

    result.append(0.0)

    else:

    result.append(1.0)

    else:

    result.append(float(item))

    dataset.append(result)

    f.close()

     

    P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0])

    print(P, P.shape)

    H_lidar = np.array([[ 1., 0., 0., 0., 0.],

    [ 0., 1., 0., 0., 0.]])

    print(H_lidar, H_lidar.shape)

     

    R_lidar = np.array([[0.0225, 0.],[0., 0.0225]])

    R_radar = np.array([[0.09, 0., 0.],[0., 0.0009, 0.], [0., 0., 0.09]])

    print(R_lidar, R_lidar.shape)

    print(R_radar, R_radar.shape)

     

    # process noise standard deviation for a

    std_noise_a = 2.0

    # process noise standard deviation for yaw acceleration

    std_noise_yaw_dd = 0.3

    • 在整个预测和测量更新过程中,所有角度量的数值都应该控制在 [−π,π],我们知道角度加减 2π 不变,

    • 所以用如下函数表示函数来调整角度:

    def control_psi(psi):

    while (psi > np.pi or psi < -np.pi):

    if psi > np.pi:

    psi = psi - 2 * np.pi

    if psi < -np.pi:

    psi = psi + 2 * np.pi

    return psi

     

    • 具体状态初始化代码为:

    state = np.zeros(5)

    init_measurement = dataset[0]

    current_time = 0.0

    if init_measurement[0] == 0.0:

    print('Initialize with LIDAR measurement!')

    current_time = init_measurement[3]

    state[0] = init_measurement[1]

    state[1] = init_measurement[2]

     

    else:

    print('Initialize with RADAR measurement!')

    current_time = init_measurement[4]

    init_rho = init_measurement[1]

    init_psi = init_measurement[2]

    init_psi = control_psi(init_psi)

    state[0] = init_rho * np.cos(init_psi)

    state[1] = init_rho * np.sin(init_psi)

    print(state, state.shape)

    • 写一个辅助函数用于保存数值:

    # Preallocation for Saving

    px = []

    py = []

    vx = []

    vy = []

     

    gpx = []

    gpy = []

    gvx = []

    gvy = []

     

    mx = []

    my = []

     

    def savestates(ss, gx, gy, gv1, gv2, m1, m2):

    px.append(ss[0])

    py.append(ss[1])

    vx.append(np.cos(ss[3]) * ss[2])

    vy.append(np.sin(ss[3]) * ss[2])

     

    gpx.append(gx)

    gpy.append(gy)

    gvx.append(gv1)

    gvy.append(gv2)

    mx.append(m1)

    my.append(m2)

    • 定义状态转移函数和测量函数,使用numdifftools库来计算其对应的雅可比矩阵,

    • 这里我们先设 Δt=0.05,只是为了占一个位置,当实际运行EKF时会计算出前后两次测量的时间差,一次来替换这里的 Δt。

    measurement_step = len(dataset)

    state = state.reshape([5, 1])

    dt = 0.05

     

    I = np.eye(5)

     

    transition_function = lambda y: np.vstack((

    y[0] + (y[2] / y[4]) * (np.sin(y[3] + y[4] * dt) - np.sin(y[3])),

    y[1] + (y[2] / y[4]) * (-np.cos(y[3] + y[4] * dt) + np.cos(y[3])),

    y[2],

    y[3] + y[4] * dt,

    y[4]))

     

    # when omega is 0

    transition_function_1 = lambda m: np.vstack((m[0] + m[2] * np.cos(m[3]) * dt,

    m[1] + m[2] * np.sin(m[3]) * dt,

    m[2],

    m[3] + m[4] * dt,

    m[4]))

     

    J_A = nd.Jacobian(transition_function)

    J_A_1 = nd.Jacobian(transition_function_1)

    # print(J_A([1., 2., 3., 4., 5.]))

     

    measurement_function = lambda k: np.vstack((np.sqrt(k[0] * k[0] + k[1] * k[1]),

    math.atan2(k[1], k[0]),

    (k[0] * k[2] * np.cos(k[3]) + k[1] * k[2] * np.sin(k[3])) / np.sqrt(k[0] * k[0] + k[1] * k[1])))

    J_H = nd.Jacobian(measurement_function)

    # J_H([1., 2., 3., 4., 5.])

    • EKF的过程代码:

    for step in range(1, measurement_step):

     

    # Prediction

    # ========================

    t_measurement = dataset[step]

    if t_measurement[0] == 0.0:

    m_x = t_measurement[1]

    m_y = t_measurement[2]

    z = np.array([[m_x], [m_y]])

     

    dt = (t_measurement[3] - current_time) / 1000000.0

    current_time = t_measurement[3]

     

    # true position

    g_x = t_measurement[4]

    g_y = t_measurement[5]

    g_v_x = t_measurement[6]

    g_v_y = t_measurement[7]

     

    else:

    m_rho = t_measurement[1]

    m_psi = t_measurement[2]

    m_dot_rho = t_measurement[3]

    z = np.array([[m_rho], [m_psi], [m_dot_rho]])

     

    dt = (t_measurement[4] - current_time) / 1000000.0

    current_time = t_measurement[4]

     

    # true position

    g_x = t_measurement[5]

    g_y = t_measurement[6]

    g_v_x = t_measurement[7]

    g_v_y = t_measurement[8]

     

    if np.abs(state[4, 0]) < 0.0001: # omega is 0, Driving straight

    state = transition_function_1(state.ravel().tolist())

    state[3, 0] = control_psi(state[3, 0])

    JA = J_A_1(state.ravel().tolist())

    else: # otherwise

    state = transition_function(state.ravel().tolist())

    state[3, 0] = control_psi(state[3, 0])

    JA = J_A(state.ravel().tolist())

     

     

    G = np.zeros([5, 2])

    G[0, 0] = 0.5 * dt * dt * np.cos(state[3, 0])

    G[1, 0] = 0.5 * dt * dt * np.sin(state[3, 0])

    G[2, 0] = dt

    G[3, 1] = 0.5 * dt * dt

    G[4, 1] = dt

     

    Q_v = np.diag([std_noise_a*std_noise_a, std_noise_yaw_dd*std_noise_yaw_dd])

    Q = np.dot(np.dot(G, Q_v), G.T)

     

    # Project the error covariance ahead

    P = np.dot(np.dot(JA, P), JA.T) + Q

     

    # Measurement Update (Correction)

    # ===============================

    if t_measurement[0] == 0.0:

    # Lidar

    S = np.dot(np.dot(H_lidar, P), H_lidar.T) + R_lidar

    K = np.dot(np.dot(P, H_lidar.T), np.linalg.inv(S))

     

    y = z - np.dot(H_lidar, state)

    y[1, 0] = control_psi(y[1, 0])

    state = state + np.dot(K, y)

    state[3, 0] = control_psi(state[3, 0])

    # Update the error covariance

    P = np.dot((I - np.dot(K, H_lidar)), P)

     

    # Save states for Plotting

    savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_x, m_y)

     

    else:

    # Radar

    JH = J_H(state.ravel().tolist())

     

    S = np.dot(np.dot(JH, P), JH.T) + R_radar

    K = np.dot(np.dot(P, JH.T), np.linalg.inv(S))

    map_pred = measurement_function(state.ravel().tolist())

    if np.abs(map_pred[0, 0]) < 0.0001:

    # if rho is 0

    map_pred[2, 0] = 0

     

    y = z - map_pred

    y[1, 0] = control_psi(y[1, 0])

     

    state = state + np.dot(K, y)

    state[3, 0] = control_psi(state[3, 0])

    # Update the error covariance

    P = np.dot((I - np.dot(K, JH)), P)

     

    savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_rho * np.cos(m_psi), m_rho * np.sin(m_psi))

    • 这里有几点需要注意,首先,要考虑清楚有几个地方被除数有可能为 0,比如说ω=0,以及 ρ=0 的情况。

    • 处理完以后我们输出估计的均方误差(RMSE),并且把各类数据保存以便我们可视化我们的EKF的效果:

    def rmse(estimates, actual):

    result = np.sqrt(np.mean((estimates-actual)**2))

    return result

     

    print(rmse(np.array(px), np.array(gpx)),

    rmse(np.array(py), np.array(gpy)),

    rmse(np.array(vx), np.array(gvx)),

    rmse(np.array(vy), np.array(gvy)))

     

    # write to the output file

    stack = [px, py, vx, vy, mx, my, gpx, gpy, gvx, gvy]

    stack = np.array(stack)

    stack = stack.T

    np.savetxt('output.csv', stack, '%.6f')

    • 最后我们来看一下我们的EKF在追踪目标的时候的均方误差:

    0.0736336090893 0.0804598933194 0.229165985264 0.309993887661

    • 我们把我们的EKF的估计结果可视化:

     

    • 我们放大一个局部看一下:

     

    • 其中,蓝色的点为我们的EKF对目标位置的估计,橙色的点为来自LIDAR和RADAR的测量值,绿色的点是目标的真实位置

    • 由此可知,我们的测量是不准确的,因此我们使用EKF在结合运动模型的先验知识以及融合两个传感器的测量的基础上做出了非常接近目标正是状态的估计。

    • EKF的魅力其实还不止在此!我们使用EKF,不仅能够对目标的状态(我们关心的)进行准确的估计,从这个例子我们会发现,EKF还能估计我们的传感器无法测量的量(比如本例中的v,ψ,ψ˙)

    • 那么,扩展卡尔曼滤波就到此结束了,大家可能会问,怎么没看到可视化结果那一部分的代码?哈哈,为了吸引一波人气,请大家关注我的博客,我会在下一期更新中(无损卡尔曼滤波)提供这一部分代码。

    • 最后,细心的同学可能会发现,我们这个EKF执行的效率太低了,实际上,EKF的一个最大的问题就是求解雅可比矩阵计算量比较大,而且相关的偏导数推导也是非常无聊的工作,因此引出了我们下一篇要讲的内容,无损卡尔曼滤波(Unscented Kalman Filter,UKF)

    • 附数据下载链接:http://download.csdn.net/download/adamshan/10033533

    参考:

    • https://blog.csdn.net/AdamShan/article/details/78265754

     
    展开全文
  • #include #include#include#include#includeusing namespacemrpt;using namespacemrpt::bayes;using namespacemrpt::math;using namespacemrpt::utils;using namespacemrpt::random;using namespacestd;...
  • 基于卡尔曼滤波的三种经典室内定位算法:Fang、Chan、Taylor,MATLAB仿真。
  • 参考:https://blog.csdn.net/young_gy/article/details/78468153Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文...
  • 扩展卡尔曼滤波(EKF)理论讲解与实例(matlab、python和C++代码) 文章目录扩展卡尔曼滤波(EKF)理论讲解与实例(matlab、python和C++代码)理论讲解KF和EKF模型对比雅可比矩阵计算计算实例应用实例线性模型CV模型...
  • 03TDOAAOA定位的扩展卡尔曼滤波算法MATLAB源代码 欢迎访问 GreenSim 团队主页→ 邮箱:greensim@第 1 页TDOA/AOA定位的扩展卡尔曼滤波算法 MATLAB源代码TDOA/AOA 是无线定位领域里使用得比较多的一种定位体制,扩展...
  • 卡尔曼滤波python及陀螺仪例子

    千次阅读 2018-10-05 21:18:38
    卡尔曼滤波在处理信号的噪声方面及其有用,最近看到一篇博客,讲解的通俗易懂,就不重复阐述了。附上个地址:卡尔曼滤波,通俗易懂   import matplotlib.pyplot as plt import numpy as np #创建噪声 NUM = ...
  • 拓展卡尔曼滤波学习(python源码)

    千次阅读 2020-09-10 11:42:11
    拓展卡尔曼滤波的逐步理解与实现 这个文章讲的非常不错。配套代码实现文章。 【机器人位置估计】卡尔曼滤波的原理与实现 本文主要是针对两篇文章的基础上做笔记和记录学习过程。 一、基本模型 1.1 机器人小M ...
  • 点击上方“小白学视觉”,选择加"星标"或“置顶”重磅干货,第一时间送达EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统,EKF生成的状态估计比仅使用实际测量值更准确。在本文中,...
  • 1. 使用卡尔曼滤波器的目的 我们假设你建造了一个可以在树林里行走的小机器人,为了精准的进行导航,机器人需要每时每刻都知道它自己的位置 我们用符号来表示机器人的状态变量,在此处我们假设状态变量只包含...
  • 扩展卡尔曼滤波新手教程(四)----中文版

    千次阅读 多人点赞 2019-05-19 21:26:57
    扩展卡尔曼滤波新手教程(四) 说明:本文内容翻译自外文网站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts,仅供学习和参考。 本文是扩展卡尔曼滤波新手教程系列的第四篇,也是最后一篇,...
  • 扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分

    千次阅读 多人点赞 2021-03-30 16:26:02
    扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分 算法部分见博客: 扩展卡尔曼滤波EKF在目标跟踪中的应用—算法部分 https://blog.csdn.net/weixin_44044161/article/details/115313573 作者:823618313@qq.com 备注...
  • 本期由东南大学仪器科学与技术博士魏宏宇分享,分享的主题为《视觉惯性SLAM之多约束扩展卡尔曼滤波》,主讲人会对该领域的核心和主流技术进行详细讲解,欢迎大家参与线上讨论。 我们举办的线上直播分享,旨在更加...
  • 前言:上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果。所以之前为了保证我们的处理模型是线性的,...
  • # coding=utf-8 import numpy as np import numdifftools as nd import math dataset = [] # read the measurement data, use 0.0 to stand LIDAR da...
  • 卡尔曼滤波与目标追踪

    千次阅读 2020-10-13 00:39:48
    扩展卡尔曼滤波(EKF)与传感器融合 过程模型,无损卡尔曼滤波(UKF)与车辆状态轨迹 为什么要学卡尔曼滤波? 卡尔曼滤波以及其扩展算法能够应用于目标状态估计,如果这个目标是行人,那么就是行人状态估计(或者说...
  • 在本文中,我将演示使用EKF(扩展卡尔曼滤波)对四元数确定姿态的实现,并说明将多个传感器数据融合在一起以使系统正常工作的必要性。 将要使用的传感器是陀螺仪,加速度计和磁力计。 Arduino用于从传感器读取数据,...
  • 对加速度传感器和陀螺仪的数据处理,然后卡尔曼滤波得出角度与角速度;.
  • 卡尔曼滤波在目标跟踪中的运用

    千次阅读 2021-10-05 21:15:11
    卡尔曼滤波在目标跟踪中的运用 文章目录卡尔曼滤波在目标跟踪中的运用一、算法简述二、算法实践1. 手写卡尔曼滤波器2. 调用opencv自带的卡尔曼滤波器总结参考 一、算法简述   卡尔曼滤波(Kalman filtering)是...
  • Python中的卡尔曼滤波器 这是Kalman过滤器如何在Python中工作的基本示例。 我确实计划在将来重构和扩展此存储库。 我一直关注的有关卡尔曼滤波器的系列文章可以在找到。 我正在使用的示例也可以在同一视频中找到。 ...
  • 这是C ++中扩展卡尔曼滤波器实现,用于融合激光雷达和雷达传感器测量。 卡尔曼滤波器可用于任何不确定某个动态系统信息的地方,并且您想对系统下一步将要做什么做出有根据的猜测。 在这种情况下,我们有两个...
  • 扩展卡尔曼滤波的实践卡尔曼滤波的推导 概率机器人总结——(扩展)卡尔曼滤波先实践再推导 为什么要把扩展两个字加个括号呢,因为本文的实践过程是以扩展卡尔曼为例,但是推导过程主要是参考博客卡尔曼滤波 – 从推导...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 606
精华内容 242
关键字:

扩展卡尔曼滤波python

python 订阅