【Python仿真】基于EKF的传感器融合定位

基于EKF的传感器融合定位（Python仿真）

  • 简述
  • 1. 背景介绍
    • 1.1. EKF扩展卡尔曼滤波
      • 1.1.1.概念
      • 1.1.2. 扩展卡尔曼滤波的主要步骤如下：
      • 1.1.3. 优、缺点
    • 1.2. 航位推算
    • 1.3. 目前航位算法的使用通常与卡尔曼滤波相结合使用
      • 2. 分段代码
    • 2.1. 导入需要的包
    • 2.2. 设置相关参数
    • 2.3. 输入
    • 2.4. 噪声添加
    • 2.5. 运动模型
    • 2.6. 观测模型
    • 2.7. 雅可比（Jacobian）运动模型
    • 2.8. 扩展卡尔曼滤波估计模型
      • 2.8.1. 预测
    • 2.9. 计算并绘制EKF协方差椭圆
    • 2.10. 主函数
  • 3. 代码运行结果展示与分析
    • 3.1. 实验结果展示
    • 3.2. EKF与航位推算的比较
      • 3.2.1. 非线性系统
      • 3.2.2. 高精度估计
      • 3.2.3. 适应不确定性
      • 3.2.4. 实时性

简述

用Python代码实现EKF的方法对比航位推算的结果，表明EKF的融合定位精度比单纯使用航位推算的精度更高。

1. 背景介绍

1.1. EKF扩展卡尔曼滤波

1.1.1.概念

卡尔曼滤波（Kalman Filtering）是一种用于估计系统状态的递归滤波方法，广泛应用于信号处理、控制系统、机器人技术等领域。扩展卡尔曼滤波（Extended Kalman Filtering）是卡尔曼滤波的一个扩展版本，用于非线性系统的状态估计。
在扩展卡尔曼滤波中，系统被建模为非线性动态系统，其中状态由一个非线性函数描述，并且状态的观测值受到高斯噪声的影响。扩展卡尔曼滤波通过线性化非线性函数来近似系统的动态特性，并利用卡尔曼滤波的递归算法来估计系统的状态。

1.1.2. 扩展卡尔曼滤波的主要步骤如下：

  • 初始化：设置系统的初始状态和协方差矩阵。
  • 预测：根据系统的动态模型和当前状态的估计值，预测下一个时刻的状态和协方差矩阵。
  • 线性化：将非线性函数线性化为一个雅可比矩阵，用于计算卡尔曼增益。
  • 更新：根据观测值和预测的状态值，计算卡尔曼增益，并更新状态的估计值和协方差矩阵。

1.1.3. 优、缺点

扩展卡尔曼滤波的优点是能够处理非线性系统，并且具有较好的估计性能。
然而，它对初始状态的估计要求较高，并且线性化过程可能引入估计误差。对于非线性程度较高的系统，线性化可能导致估计误差增大。
因此，在应用扩展卡尔曼滤波时，需要根据实际问题进行参数调整和误差分析，以保证滤波器的性能。

1.2. 航位推算

航位推算（Dead Reckoning）是一种在航海和航空中用于确定当前位置的方法。
其原理基于以下几个假设：

  • 起始点位置已知：航位推算需要知道起始点的经纬度坐标。
  • 航向角和速度已知：航位推算需要知道航行器的航向角和速度。
  • 没有外部干扰：航位推算假设在航行过程中没有外部干扰，如风、水流等。

基于以上假设，航位推算的原理可以描述如下：
1 . 根据起始点位置、航向角和速度，计算出航行器在单位时间内的位移向量
2 . 将位移向量累加到起始点的经纬度坐标上，得到航行器在单位时间后的预测位置。
不断重复步骤1和2，根据航行器的实际航向角和速度更新位移向量，并累加到当前位置上，得到航行器不断更新的位置。
航位推算的原理是基于航行器的运动学原理，通过不断地预测和更新位置，可以在一定程度上确定航行器的当前位置。然而，由于航位推算没有考虑外部干扰和误差累积的问题，所以在长时间航行中可能会产生较大的误差。因此，在实际航行中，航位推算通常会与其他导航方法（如卫星导航系统）结合使用，以提高位置的准确性和可靠性。

1.3. 目前航位算法的使用通常与卡尔曼滤波相结合使用

航位推算导航系统中位置和航向角的发散特性。航位推算导航的可观测性分析表明，所设计的系统能够提供一定时间内的位置和航向角。
但是，需要通过其他系统如绝对定位系统来补偿导航误差，以延长导航时间和距离。本代码将结合扩展卡尔曼滤波实现。

2. 分段代码

2.1. 导入需要的包

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

2.2. 设置相关参数

# Estimation parameter of EKF 估计参数
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
R = np.diag([1.0, np.deg2rad(40.0)])**2
#  Simulation parameter 仿真参数
Qsim = np.diag([0.5, 0.5])**2
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
DT = 0.1  # time tick [s] 时间刻度
SIM_TIME = 50.0  # simulation time [s] 模拟时间
show_animation = True

2.3. 输入

def calc_input():
    v = 1.0  # [m/s]
    yawrate = 0.1  # [rad/s]  偏航角速率
    u = np.matrix([v, yawrate]).T
    return u

2.4. 噪声添加

def observation(xTrue, xd, u):

    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y  添加噪声到GPS x-y
    zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
    zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
    z = np.matrix([zx, zy])

    # add noise to input 给输入加噪
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.matrix([ud1, ud2]).T

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud

2.5. 运动模型

def motion_model(x, u):

    F = np.matrix([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])

    B = np.matrix([[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

2.6. 观测模型

def observation_model(x):
    #  Observation Model
    H = np.matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    z = H * x

    return z

2.7. 雅可比（Jacobian）运动模型

需要注意的是，雅可比运动模型是一种简化的模型，它基于一些假设和近似，可能不能完全准确地描述实际情况。然而，它仍然是解释群体扩散和迁移的有用工具，并且可以通过调整参数和引入更复杂的扩展模型来提高其准确性。

def jacobF(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.matrix([
        [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 jacobH(x):
    # Jacobian of Observation Model 观测模型的雅可比矩阵
    jH = np.matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])
    return jH

2.8. 扩展卡尔曼滤波估计模型

2.8.1. 预测

  • 状态预测
    系统模型：假设非线性系统的状态方程可以表示为 xPred = f(xEst ,u) + z，其中 x_k 是当前时刻的状态向量，f() 是非线性函数，u是当前时刻的控制输入，z是过程噪声。
    预测状态：通过对系统模型进行线性化近似，使用雅可比矩阵（Jacobian Matrix）F_k 来近似表示状态方程：xPred = f̂(xEst, u)，其中 xPred是预测的状态估计值，f̂() 是线性化后的状态更新函数。
  • 协方差预测
    预测协方差：使用雅可比矩阵 jF进行线性化近似，通过更新方程 PPred = jF * PEst * jF.T +Q 来计算预测的协方差矩阵，其中 PPred 是预测的协方差矩阵。
    测量模型：假设非线性系统的测量方程可以表示为 zPred = H * xPred其中 z_k 是当前时刻的测量向量，h() 是非线性函数，v_k 是测量噪声。
    预测测量：通过对测量模型进行线性化近似，使用雅可比矩阵 jH来近似表示测量方程：zPred = jH * xPred，其中 zPred是预测的测量值，H 是线性化后的测量函数。
    残差计算：计算残差向量 y = z.T – zPred 。
    残差协方差：假设测量噪声服从高斯分布，通过测量噪声协方差矩阵 R 来描述测量噪声的方差。
    估计卡尔曼增益：通过雅可比矩阵 jH 进行线性化近似，计算卡尔曼增益 K = PPred * jH.T * np.linalg.inv(S)，其中 S = jH * PPred * jH.T + R 。
    更新状态估计值：使用卡尔曼增益将预测的状态估计值修正为最终的状态估计值 xEst = xPrede + K * y。
    更新协方差矩阵：使用卡尔曼增益将预测的协方差矩阵修正为最终的协方差矩阵 PEst = (I - K * jH) * Pred ，其中 I 是单位矩阵。
def ekf_estimation(xEst, PEst, z, u):

    #  Predict
    xPred = motion_model(xEst, u)
    jF = jacobF(xPred, u)
    PPred = jF * PEst * jF.T + Q

    #  Update
    jH = jacobH(xPred)
    zPred = observation_model(xPred)
    y = z.T - 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

2.9. 计算并绘制EKF协方差椭圆

def plot_covariance_ellipse(xEst, PEst):    # EKF估计的协方差椭圆
    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[bigind, 1], eigvec[bigind, 0])
    R = np.matrix([[math.cos(angle), math.sin(angle)],
                   [-math.sin(angle), math.cos(angle)]])
    fx = R * np.matrix([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")

2.10. 主函数

def main():
    print(__file__ + " start!!")

    time = 0.0

    # State Vector [x y yaw v]' 状态向量
    xEst = np.matrix(np.zeros((4, 1)))
    xTrue = np.matrix(np.zeros((4, 1)))
    PEst = np.eye(4)

    xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning 船位推算

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((1, 2))

    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.vstack((hz, z))

        if show_animation:
		plt.rcParams['axes.unicode_minus'] = False
            plt.rcParams['font.sans-serif'] = ['SimHei']    #matplotlib.pyplot绘图显示中文
            plt.cla()
            plt.plot(hz[:, 0], hz[:, 1], ".g",label="定位观测点")  # 绿点为定位观测(如GPS)
            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b",
                     label="真实轨迹")    # 蓝线为真实轨迹
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k",label="航位推算轨迹")  # 黑线为航位推算轨迹
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r",label="EKF估计轨迹") # 红线为EKF估计轨迹
            plot_covariance_ellipse(xEst, PEst)
            plt.legend()
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

3. 代码运行结果展示与分析

3.1. 实验结果展示

可以看出一开始航位推算的效果要优于EKF，是因为EKF还处于一个更新调整的阶段，随着时间的推进，航位推算的轨迹与真实的蓝色轨迹相差越来越大，红色的EKF轨迹与真实轨迹之间有误差，但在一定小的一个范围内。图中绿色的点是GPS的观测点，选取一个固定范围内的点来更新预测EKF的轨迹。
在这里插入图片描述

3.2. EKF与航位推算的比较

3.2.1. 非线性系统

船位推算通常涉及到非线性状态方程，如运动模型。
而扩展卡尔曼滤波能够通过对非线性系统进行线性化，使得可以在非线性系统中进行状态估计。

3.2.2. 高精度估计

扩展卡尔曼滤波通过在每个时间步骤上更新状态估计和协方差矩阵，能够提供对船位的高精度估计。通过不断的测量和预测更新过程，可以减小估计误差并产生更准确的船位估计结果。

3.2.3. 适应不确定性

扩展卡尔曼滤波能够处理系统和测量的不确定性。在船位推算中，存在各种误差来源，如传感器误差、环境影响等，扩展卡尔曼滤波能够通过动态调整协方差矩阵来适应这些不确定性，从而提供更鲁棒的航位估计。

3.2.4. 实时性

扩展卡尔曼滤波具有较高的计算效率和实时性，适用于需要实时船位推算的场景。
通过有效的算法设计和优化，扩展卡尔曼滤波能够在短时间内完成状态估计，适用于高频率的航位推算任务。

代码链接：GitHub - UI-Mario/EKF: 扩展卡尔曼滤波/ Extended Kalman Filter(EKF)

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mfbz.cn/a/165742.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Linux操作文件的底层系统调用

Linux操作文件的底层系统调用

目录 1.概述 2.open的介绍 3.write 的介绍 4.read 5.close的介绍 6.文件描述符 1.概述 C语言操作文件的几个库函数:fopen,fread,fwrite,fclose; 系统调用:open,read,write,close; 系统调用方法实现在内核中;(陷入内核,切换到内核) 2.open的介绍 open重载:两个参数用于打…
阅读更多...
【0到1学习Unity脚本编程】第一人称视角的角色控制器

【0到1学习Unity脚本编程】第一人称视角的角色控制器

👨‍💻个人主页：元宇宙-秩沅 👨‍💻 hallo 欢迎 点赞👍 收藏⭐ 留言📝 加关注✅! 👨‍💻 本文由 秩沅 原创 👨‍💻 收录于专栏：【0…
阅读更多...
【Gradle-13】SNAPSHOT版本检查

【Gradle-13】SNAPSHOT版本检查

1、什么是SNAPSHOT SNAPSHOT版本是指尚未发布的版本，是一个「动态版本」，它始终指向最新的发布工件（gav），也就是说同一个SNAPSHOT版本可以反复用来发布。 这种情况在大型app多团队的开发中比较常见，比如us…
阅读更多...
ROS参数服务器(Param)：通信模型、Hello World与拓展

ROS参数服务器(Param)：通信模型、Hello World与拓展

参数服务器在ROS中主要用于实现不同节点之间的数据共享。 参数服务器相当于是独立于所有节点的一个公共容器，可以将数据存储在该容器中，被不同的节点调用，当然不同的节点也可以往其中存储数据。 使用场景一般存储一些机器人的固有参数&…
阅读更多...
系列四、强引用、软引用、弱引用、虚引用分别是什么？

系列四、强引用、软引用、弱引用、虚引用分别是什么？

一、整体架构 二、强引用（默认支持模式） 2.1、概述 当内存不足时，JVM开始垃圾回收，对于强引用的对象，就算是出现了OOM也不会对该对象进行回收，死都不收。 强引用是我们最常见的普通对象引用，只…
阅读更多...
Gin框架源码解析

Gin框架源码解析

概要 目录 Gin路由详解 Gin框架路由之Radix Tree 一、路由树节点 二、请求方法树 三、路由注册以及匹配 中间件含义 Gin框架中的中间件 主要讲述Gin框架路由和中间件的详细解释。本文章将从Radix树（基数树或者压缩前缀树）、请求处理、路由方法树…
阅读更多...
hypermesh学习总结（一）

hypermesh学习总结（一）

1、hypermesh导入导出 2、hypermesh如何使用拓扑命令，连接多个几何体为一个？ 3、hypermesh模式选择 分别有显示动力学模式explicit,标准模式Standard3D(静力学及模态等)
阅读更多...
Linux之进程概念（一）

Linux之进程概念（一）

📘北尘_：个人主页 🌎个人专栏:《Linux操作系统》《经典算法试题 》《C》 《数据结构与算法》 ☀️走在路上，不忘来时的初心 文章目录 一、冯诺依曼体系结构二、操作系统(Operator System)1、概念2、设计OS的目的3、定位4、如何理…
阅读更多...
2024年csdn最新最全的Postman接口测试： postman实现参数化

2024年csdn最新最全的Postman接口测试： postman实现参数化

什么时候会用到参数化 比如：一个模块要用多组不同数据进行测试 验证业务的正确性 Login模块：正确的用户名，密码 成功；错误的用户名，正确的密码 失败 postman实现参数化 在实际的接口测试中，部分参数…
阅读更多...
弗洛伊德算法（C++）

弗洛伊德算法（C++）

目录 介绍： 代码： 结果： 介绍： 弗洛伊德算法（Floyd algorithm）也称为Floyd-Warshall算法，是一种用于求解所有节点对之间的最短路径的动态规划算法。它使用了一个二维数组来存储所有节点…
阅读更多...
深入解析具名导入es6规范中的具名导入是在做解构吗

深入解析具名导入es6规范中的具名导入是在做解构吗

先说答案，不是 尽管es6的具名导入和语法非常相似 es6赋值解构 const obj {a: 1,f() {this.a}}const { a, f } objes6具名导入 //导出文件代码export let a 1export function f() {a}export default {a,f}//导入文件代码import { a, f } from ./tsVolution可以看出…
阅读更多...
Unity2021及以上 启动或者禁用自动刷新

Unity2021及以上 启动或者禁用自动刷新

Unity 2021以以上启动自动刷新 Edit---> Preferences--> Asset Pipline --> Auto Refresh 禁用的结果 如果不启动自动刷新在Project面板选择Refresh是不会刷新已经修改后的脚本的。
阅读更多...
10_6 input输入子系统,流程解析

10_6 input输入子系统,流程解析

简单分层 应用层 内核层 --------------------------- input handler 数据处理层 driver/input/evdev.c1.和用户空间交互,实现fops2.不知道数据怎么得到的,但是可以把数据上传给用户--------------------------- input core层1.维护上面和下面的两个链表2.为上下两层提供接口--…
阅读更多...
智慧路灯控制系统设计方案思路及设计原则

智慧路灯控制系统设计方案思路及设计原则

智慧路灯系统依托于智慧路灯综合管理平台，实现点（智慧路灯）、线（道路）、面（城市）的三级监控，实现灯控、屏控、视频监控、数据采集、联动的统一。 1）一个城市的智慧路灯系…
阅读更多...
Shell判断：流程控制—if（三）

Shell判断：流程控制—if（三）

一、调试脚本 1、调试脚本的其他方法： [rootlocalhost ~] # sh -n useradd.sh 仅调试脚本中的语法错误。 [rootlocalhost ~]# sh -vx useradd.sh 以调试的方式执行，查询整个执行过程。 2、示例： [rootlocalhost ~]# sh -n useradd.sh #调…
阅读更多...
【数据结构＆C++】二叉平衡搜索树-AVL树（25）

【数据结构＆C++】二叉平衡搜索树-AVL树（25）

前言 大家好吖，欢迎来到 YY 滴C系列 ，热烈欢迎！ 本章主要内容面向接触过C的老铁 主要内容含： 欢迎订阅 YY滴C专栏！更多干货持续更新！以下是传送门！ 目录 一.AVL树的概念二.AVL树节点的定义(代码…
阅读更多...
OpenHarmony源码下载

OpenHarmony源码下载

OpenHarmony源码下载 现在的 OpenHarmony 4.0 源码已经有了，在 https://gitee.com/openharmony 地址中，描述了源码获取的方式，但那是基于 ubuntu 或者说是 Linux 的下载方式。在 windows 平台下的下载方式没有做出介绍。 我自己尝试了 wind…
阅读更多...
linux文件IO

linux文件IO

文件IO截断 截断对文件的偏移量没有影响。
阅读更多...
【Go入门】 Go如何使得Web工作

【Go入门】 Go如何使得Web工作

【Go入门】 Go如何使得Web工作 前面小节介绍了如何通过Go搭建一个Web服务，我们可以看到简单应用一个net/http包就方便的搭建起来了。那么Go在底层到底是怎么做的呢？万变不离其宗，Go的Web服务工作也离不开我们第一小节介绍的Web工作方式。 w…
阅读更多...
简单聊一聊幂等和防重

简单聊一聊幂等和防重

大家好，我是G探险者。 每年的双十一，618，电商系统都会面临这超高的流量，如果一个订单被反复提交，那电商系统如何保证这个订单之后执行一次减库存，扣款的操作？ 这里就引入两个概念，…
阅读更多...
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站