10.15
Allan方差
理论参考：严恭敏老师《惯性仪器测试与数据分析》第八章
Allan 方差分析以及python代码实现_allan方差分析-CSDN博客
阿伦方差（Allen variance）是一种用于评估时间序列数据（例如传感器数据）稳定性和噪声特性的统计量。适用于信号噪声分析和频率稳定性测量。常用于时频分析和惯性导航。也可应用于任何时间序列的分析。
典型的六轴IMU输出三个轴的加速度和三个轴的角速度。对加速度做积分可以得到速度、再做一次积分可以得到位移；对角速度积分可以得到角度。
高精度陀螺的Allan方差示意图：
所需参数
-
时间序列数据：原始数据序列，例如 IMU 的加速度或角速度数据。
-
时间间隔（τ）：用于分段的时间长度，通常需要根据应用场景进行选择。
-
段的数量（M）：可以从时间序列的总长度和时间间隔推算出。
计算过程
阿伦方差通过将时间序列数据分成若干段，然后计算每段的均值，再评估这些均值之间的变化，从而反映信号的稳定性和噪声特性。
通过段均值的变化（差值）计算其方差，从而得到阿伦方差，这个值越小通常表示信号越稳定，噪声越小。
- 对于每个时间段，计算该段的均值：
- 计算每个段均值之间的差异：
- 阿伦方差定义为段均值差的方差：
实际在python中使用的时候发现，对数据要求不低啊，不能帧率太高时间戳太密集，数据长度不能太小。
别人的：
我的：（不是，怎么只有前半截，怎么不转头拐上去呢，是数据时间太短了吗）
python
import numpy as np
import matplotlib.pyplot as plt
import os
import sys
import math
f1 = open('./odom_file2.txt','r')
f2 = open('./imu_file2.txt','r')
odom_time = []
odom_seq = []
odom_x = []
odom_y = []
odom_vec_x = []
odom_vec_y = []
odom_vec_z = []
imu_time = []
imu_seq = []
imu_acc_x = []
imu_acc_y = []
imu_acc_z = []
imu_yaw = []
for line in f1:
l = line.split(" ")
# print(type(l),l)
try:
odom_time.append(float(l[0])/1e9)
odom_seq.append(int(l[1]))
odom_x.append(float(l[2]))
odom_y.append(float(l[3]))
odom_vec_x.append(float(l[5]))
odom_vec_y.append(float(l[6]))
odom_vec_z.append(float(l[7]))
except ValueError:
continue
for line in f2:
l = line.split(" ")
# print('-'*50)
# print(f"Processing line: {line}")
# print(f"Parsed line: {l}")
if len(l) < 6:
print("Line does not contain enough elements.")
continue
try:
imu_time.append(float(l[0])/1e9)
imu_seq.append(int(l[1]))
imu_acc_x.append(float(l[2]))
imu_acc_y.append(float(l[3]))
imu_acc_z.append(float(l[4]))
# imu_yaw.append(float(l[5])*180/math.pi)
imu_yaw.append(float(l[5]))
except ValueError:
continue
# 转换为numpy数组
imu_time = np.array(imu_time)
imu_acc_x = np.array(imu_acc_x)
imu_acc_y = np.array(imu_acc_y)
imu_acc_z = np.array(imu_acc_z)
imu_yaw = np.array(imu_yaw)
def allan_variance(data, sampling_period):
"""
计算给定数据序列的阿伦方差。
参数:
data : np.array
输入数据序列。
sampling_period : float
数据采样周期（单位：秒）。
返回:
tuple
tau : np.array
不同时长的tau值数组。
adev : np.array
对应tau值下的阿伦方差的估计值。
"""
n = len(data)
tau = []
adev = []
for m in range(1, n // 2):
tau_m = m * sampling_period
tau.append(tau_m)
# 计算平均值序列
x_bar = np.mean(data[:n-m])
y_bar = np.mean(data[m:n])
# 计算差分序列
diff = data[:n-m] - data[m:n]
# 计算阿伦方差
avar = (1 / (2 * m * tau_m)) * np.sum((diff - (x_bar - y_bar)) ** 2)
adev.append(np.sqrt(avar))
return np.array(tau), np.array(adev)
# 假设imu_time是采样时间戳，我们计算采样周期
sampling_period = np.mean(np.diff(imu_time))
# 计算每个轴的阿伦方差
tau_x, adev_x = allan_variance(imu_acc_x, sampling_period)
tau_y, adev_y = allan_variance(imu_acc_y, sampling_period)
tau_z, adev_z = allan_variance(imu_acc_z, sampling_period)
plt.figure()
plt.loglog(tau_x, adev_x, label='X Axis')
plt.loglog(tau_y, adev_y, label='Y Axis')
plt.loglog(tau_z, adev_z, label='Z Axis')
plt.xlabel('Tau (s)')
plt.ylabel('Allan Deviation')
plt.title('Allan Variance of Accelerometer')
plt.legend()
plt.grid(True, which="both", ls="--")
plt.show()
# import numpy as np
# import matplotlib.pyplot as plt
# tau = np.logspace(-3, 1, 100) # 观测时间，从毫秒级到秒级
# allan_dev = 1 / (tau + 1) # 模拟的正常阿兰偏差，仅为示例
# white_noise_slope = tau / 100 # 模拟的白噪声斜率，仅为示例
# # 绘制正常阿兰偏差
# plt.figure(figsize=(10, 5))
# plt.subplot(1, 2, 1)
# plt.loglog(tau, allan_dev, marker='o', linestyle='-')
# plt.xlabel('Observation Time (s)')
# plt.ylabel('Allan Deviation (σ)')
# plt.title('Normal Allan Deviation of Gyroscope')
# plt.grid(True, which="both", ls="--")
# # 绘制白噪声斜率
# plt.subplot(1, 2, 2)
# plt.loglog(tau, white_noise_slope, marker='s', linestyle='-')
# plt.xlabel('Observation Time (s)')
# plt.ylabel('White Noise Slope (σ_b)')
# plt.title('White Noise Slope of Gyroscope')
# plt.grid(True, which="both", ls="--")
# plt.tight_layout()
# plt.show()10.16 - 10.18
IMU + Noise
>>> {"gyroscope": {
>>> "noise_density": 2.0 * 35.0 / 3600.0 / 180.0 * pi,
>>> "random_walk": 2.0 * 4.0 / 3600.0 / 180.0 * pi,
>>> "bias_correlation_time": 1.0e3,
>>> "turn_on_bias_sigma": 0.5 / 180.0 * pi},
>>> "accelerometer": {
>>> "noise_density": 2.0 * 2.0e-3,
>>> "random_walk": 2.0 * 3.0e-3,
>>> "bias_correlation_time": 300.0,
>>> "turn_on_bias_sigma": 20.0e-3 * 9.8
>>> },
>>> "update_rate": 1.0} # Hz
GPS + Noise
>>> {"fix_type": 3, # 定位类型
>>> "eph": 1.0, # 水平精度因子
>>> "epv": 1.0, # 垂直精度因子
>>> "sattelites_visible": 10, # 可见卫星数量，用于评估定位精度和可靠性
>>> "gps_xy_random_walk": 2.0, # (m/s) / sqrt(hz) 水平随机漂移速率
>>> "gps_z_random_walk": 4.0, # (m/s) / sqrt(hz) 垂直随机漂移速率
>>> "gps_xy_noise_density": 2.0e-4, # (m) / sqrt(hz) 水平噪声密度
>>> "gps_z_noise_density": 4.0e-4, # (m) / sqrt(hz) 垂直噪声密度
>>> "gps_vxy_noise_density": 0.2, # (m/s) / sqrt(hz) 水平速度噪声密度
>>> "gps_vz_noise_density": 0.4, # (m/s) / sqrt(hz) 垂直速度噪声密度
>>> "gps_correlation_time": 60, # s 噪声相关时间，影响位置和速度估计的时间延迟
>>> "update_rate": 1.0 # Hz 更新频率
>>> }
latitude: 原始纬度转换为弧度。
longitude: 原始经度转换为弧度。
altitude: 原始高度（海拔）。
eph: 水平精度因子，表示水平定位的精度。
epv: 垂直精度因子，表示垂直定位的精度。
speed: 速度，初始值为0.0。
velocity_north: 北向速度，初始值为0.0。
velocity_east: 东向速度，初始值为0.0。
velocity_down: 向下的速度，初始值为0.0。
fix_type: 定位类型，来自类中的 _fix_type。
cog: 课程角（Course Over Ground），初始值为0.0。
sattelites_visible: 可见卫星数量，来自类中的 _sattelites_visible。
latitude_gt: 真实纬度（ground truth），转换为弧度。
longitude_gt: 真实经度（ground truth），转换为弧度。
altitude_gt: 真实高度（ground truth）。
ODOM
（不知道对不对呢，错了之后再改）
目的：仿真odom位置和速度数据 ------> GPS纬度、经度和高度等大地坐标系数据
因为仿真odom数据可以看作位置真值，以仿真世界原点为中心/开始运动时的坐标为中心（目前是从哪里开始运行这个世界，哪里就是中心），局部坐标系。需要转为以地球中心为原点的地心固定坐标系，再转换为大地坐标系，理论上就获得了GPS格式的位置真值信息。
GPS位置真值+高斯白噪声+随机游走 = 可以使用的GPS信息
从本地坐标系到 ECEF 坐标系
(1)计算ECEF坐标
其中 𝜆 是原点的经度
(2)法线
其中 𝑎是椭球体的长半径（地球赤道半径(m)），𝑒是第一扁率（地球在两极与赤道之间的扁平程度），𝜙是原点的纬度（a=6378137.0；e=1/298.257223563）
从 ECEF 坐标系到大地坐标系
有了 ECEF 坐标后，就可以将其转换为大地坐标系中的纬度、经度和高度。
(1)计算纬度 𝜙
(2)计算经度 𝜆
(3)计算高度 ℎ
构造GPS信息字典，放入相关参数
gps_noise.py:接收/odom rostopic ，转为gps格式，添加噪声
python
'''
| gps_noise.py
| 10.18
| python3 ./gps_noise.py
| 接收来自 Isaac Sim 的 rostopic 信息: /odom
| 转换为 GPS 格式的信息:局部坐标系转 ECEF 坐标系,再转换为大地坐标系
| 对 GPS 格式的 odom 信息进行处理加噪声
| 发送添加了噪声的 rostopic:/gps (仅发送了位置信息，速度信息不知道需不需要)
'''
__all__ = ["GPS"]
import sys
from os.path import abspath, join, dirname
sys.path.insert(0, join(abspath(dirname(__file__)), 'utils'))
import numpy as np
from utils.sensor import Sensor
from utils.geo_mag_utils import reprojection
from utils.param_config import _config_gps
from utils.state import State
# 接收 Isaac Sim rostopic: /odom
import rospy
from nav_msgs.msg import Odometry
# 发送 rostopic: /gps
from sensor_msgs.msg import NavSatFix, NavSatStatus
# 在GPS中引入延迟
# TODO - Introduce delay on the GPS data
# --------------------------------------------------------------------------------
# Define GPS data and add noise
# --------------------------------------------------------------------------------
class GPS(Sensor):
def __init__(self, config):
super().__init__(sensor_type="GPS", update_rate=config.update_rate)
self._fix_type = config.fix_type
self._eph = config.eph
self._epv = config.epv
self._sattelites_visible = config.sattelites_visible
self._random_walk_gps= np.array([0.0, 0.0, 0.0])
self._gps_xy_random_walk = config.gps_xy_random_walk
self._gps_z_random_walk = config.gps_z_random_walk
self._noise_gps_pos = np.array([0.0, 0.0, 0.0])
self._gps_xy_noise_density = config.gps_xy_noise_density
self._gps_z_noise_density = config.gps_z_noise_density
self._noise_gps_vel = np.array([0.0, 0.0, 0.0])
self._gps_vxy_noise_density = config.gps_vxy_noise_density
self._gps_vz_noise_density = config.gps_vz_noise_density
self._gps_bias = np.array([0.0, 0.0, 0.0])
self._gps_correlation_time = config.gps_correlation_time
self.position = np.array([0.0, 0.0, 0.0])
# 初始化状态属性
self.latitude = None
self.longitude = None
self.altitude = None
self._state = {
"latitude": np.radians(self.latitude or self._origin_lat),
"longitude": np.radians(self.longitude or self._origin_lon),
"altitude": self.altitude or self._origin_alt,
"eph": 1.0,
"epv": 1.0,
"speed": 0.0,
"velocity_north": 0.0,
"velocity_east": 0.0,
"velocity_down": 0.0,
# 常量值
"fix_type": self._fix_type,
"eph": self._eph,
"epv": self._epv,
"cog": 0.0,
"sattelites_visible": self._sattelites_visible,
"latitude_gt": np.radians(self.latitude or self._origin_lat),
"longitude_gt": np.radians(self.longitude or self._origin_lon),
"altitude_gt": self.altitude or self._origin_alt,
}
def gps_callback(self, gps_message):
"""回调函数，处理接收到的 GPS 数据"""
# 位置信息
self.latitude = gps_message["latitude"]
self.longitude = gps_message["longitude"]
self.altitude = gps_message["altitude"]
# position = [gps_message.get("east", 0.0), gps_message.get("north", 0.0), gps_message.get("up", 0.0)]
# 线速度
self.linear_velocity = np.array([
gps_message["velocity_east"],
gps_message["velocity_north"],
gps_message["velocity_down"]
])
# self._state["speed"] = np.linalg.norm(list(self.linear_velocity.values()))
# 更新 position 属性
self.position = np.array([self.latitude, self.longitude, self.altitude])
self.update_state(
latitude=self.latitude,
longitude=self.longitude,
altitude=self.altitude,
)
def update_state(self, latitude, longitude, altitude):
"""更新GPS位置信息"""
self._state["latitude"] = latitude
self._state["longitude"] = longitude
self._state["altitude"] = altitude
self.position = np.array([latitude, longitude, altitude])
self._state["position"] = self.position.tolist()
# @property
def state(self):
return self._state
@Sensor.update_at_rate
def update(self, state: np.ndarray, dt: float):
self._random_walk_gps[0] = self._gps_xy_random_walk * np.sqrt(dt) * np.random.randn()
self._random_walk_gps[1] = self._gps_xy_random_walk * np.sqrt(dt) * np.random.randn()
self._random_walk_gps[2] = self._gps_z_random_walk * np.sqrt(dt) * np.random.randn()
self._noise_gps_pos[0] = self._gps_xy_noise_density * np.sqrt(dt) * np.random.randn()
self._noise_gps_pos[1] = self._gps_xy_noise_density * np.sqrt(dt) * np.random.randn()
self._noise_gps_pos[2] = self._gps_z_noise_density * np.sqrt(dt) * np.random.randn()
self._noise_gps_vel[0] = self._gps_vxy_noise_density * np.sqrt(dt) * np.random.randn()
self._noise_gps_vel[1] = self._gps_vxy_noise_density * np.sqrt(dt) * np.random.randn()
self._noise_gps_vel[2] = self._gps_vz_noise_density * np.sqrt(dt) * np.random.randn()
self._gps_bias[0] = (
self._gps_bias[0] + self._random_walk_gps[0] * dt - self._gps_bias[0] / self._gps_correlation_time
)
self._gps_bias[1] = (
self._gps_bias[1] + self._random_walk_gps[1] * dt - self._gps_bias[1] / self._gps_correlation_time
)
self._gps_bias[2] = (
self._gps_bias[2] + self._random_walk_gps[2] * dt - self._gps_bias[2] / self._gps_correlation_time
)
pos_with_noise: np.ndarray = self.position + self._noise_gps_pos + self._gps_bias
latitude, longitude = reprojection(pos_with_noise, np.radians(self._origin_lat), np.radians(self._origin_lon))
latitude_gt, longitude_gt = reprojection(
self.position , np.radians(self._origin_lat), np.radians(self._origin_lon)
)
# velocity加噪声
# velocity: np.ndarray = state.linear_velocity #+ self._noise_gps_vel
velocity: np.ndarray = self.linear_velocity + self._noise_gps_vel
speed: float = np.linalg.norm(velocity[:2])
print(f'velocity : {velocity}')
print(f'velocity speed : {speed}')
ve = velocity[0]
vn = velocity[1]
cog = np.degrees(np.arctan2(ve, vn))
if cog < 0.0:
cog = cog + 360.0
cog = cog * 100
self._state = {
"latitude": np.degrees(latitude),
"longitude": np.degrees(longitude),
"altitude": self.position [2] + self._origin_alt - self._noise_gps_pos[2] + self._gps_bias[2],
"eph": 1.0,
"epv": 1.0,
"speed": speed,
# 东北天到北东地
"velocity_north": velocity[1],
"velocity_east": velocity[0],
"velocity_down": -velocity[2],
# 常量值
"fix_type": self._fix_type,
"eph": self._eph,
"epv": self._epv,
"cog": 0.0, # cog,
"sattelites_visible": self._sattelites_visible,
"latitude_gt": latitude_gt,
"longitude_gt": longitude_gt,
"altitude_gt": self.position [2] + self._origin_alt,
}
return self._state
def publish_state(self):
# 创建并发布NavSatFix消息
gps_data_msg = NavSatFix()
gps_data_msg.header.stamp = rospy.Time.now()
gps_data_msg.header.frame_id = "gps_link"
gps_data_msg.latitude = self._state["latitude"]
gps_data_msg.longitude = self._state["longitude"]
gps_data_msg.altitude = self._state["altitude"]
# 设置状态
gps_data_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
gps_data_msg.status.service = NavSatStatus.SERVICE_GPS
# 发布消息到新的topic '/gps'
self.pub_gps.publish(gps_data_msg)
# --------------------------------------------------------------------------------
# Odom data to GPS data
# --------------------------------------------------------------------------------
class OdomToGPS:
def __init__(self, origin_lat=0, origin_long=0, origin_height = 0,a=6378137.0, f=1/298.257223563):
self.origin_lat = origin_lat
self.origin_long = origin_long
self.origin_height = origin_height
self.a = a
self.f = f
self.b = a * (1 - f)
self.e_squared = 1 - (self.b**2 / self.a**2)
def local_to_ecef(self, x, y, z, lat, lon):
N = self.a / np.sqrt(1 - self.e_squared * np.sin(lat)**2)
X_ref = (N + self.origin_height) * np.cos(lat) * np.cos(lon)
Y_ref = (N + self.origin_height) * np.cos(lat) * np.sin(lon)
Z_ref = ((1 - self.e_squared) * N + self.origin_height) * np.sin(lat)
X_ECEF = X_ref + x * np.cos(lat) * np.cos(lon) - y * np.sin(lon) - z * np.sin(lat) * np.cos(lon)
Y_ECEF = Y_ref + x * np.cos(lat) * np.sin(lon) + y * np.cos(lon) - z * np.sin(lat) * np.sin(lon)
Z_ECEF = Z_ref + x * np.sin(lat) + z * np.cos(lat)
return X_ECEF, Y_ECEF, Z_ECEF
def ecef_to_geodetic(self, X_ECEF, Y_ECEF, Z_ECEF):
# 纬度
lat = np.arctan(Z_ECEF / np.sqrt(X_ECEF**2 + Y_ECEF**2))
lat_prev = None
while lat != lat_prev:
lat_prev = lat
N = self.a / np.sqrt(1 - self.e_squared * np.sin(lat)**2)
lat = np.arctan((Z_ECEF + self.e_squared * N * np.sin(lat)**3) / np.sqrt(X_ECEF**2 + Y_ECEF**2))
# 经度
lon = np.arctan2(Y_ECEF, X_ECEF)
# 高度
height = np.sqrt(X_ECEF**2 + Y_ECEF**2) / np.cos(lat) - N
return np.degrees(lat), np.degrees(lon), height
def convert_odom_to_gps(self, odom_data, gps):
self.gps = gps
# 提取位置信息
x = odom_data['pose']['pose']['position']['x']
y = odom_data['pose']['pose']['position']['y']
z = odom_data['pose']['pose']['position']['z']
# 将局部坐标系中的位置转换为ECEF坐标系（笛卡尔坐标系）
X_ECEF, Y_ECEF, Z_ECEF = self.local_to_ecef(
x, y, z,
np.radians(self.origin_lat),
np.radians(self.origin_long)
)
# 将ECEF坐标系中的位置转换为大地坐标系
lat, lon, height = self.ecef_to_geodetic(X_ECEF, Y_ECEF, Z_ECEF)
# 构造GPS消息
gps_message = {
"latitude": lat,
"longitude": lon,
"altitude": height,
"velocity_north": odom_data['twist']['twist']['linear']['y'],
"velocity_east": odom_data['twist']['twist']['linear']['x'],
"velocity_down": -odom_data['twist']['twist']['linear']['z'],
"timestamp": odom_data['header']['stamp']['secs'] + odom_data['header']['stamp']['nsecs'] * 1e-9,
"fix_type": 3,
"eph": 1.0,
"epv": 1.0,
"cog": np.degrees(odom_data['twist']['twist']['angular']['z']),
"satellites_visible": 10
}
self.latitude = lat
self.longitude = lon
self.altitude = height
# 提取线速度信息
linear_velocity = {
'velocity_east': odom_data['twist']['twist']['linear']['x'],
'velocity_north': odom_data['twist']['twist']['linear']['y'],
'velocity_down': odom_data['twist']['twist']['linear']['z']
}
print('--'*50)
print(linear_velocity)
self.linear_velocity = linear_velocity
self.gps.gps_callback(gps_message)
# self.gps.update(State(), 0.01)
gps.publish_state()
return gps_message
# --------------------------------------------------------------------------------
# Publish topic : /gps , frame_id = 'gps_link'
# --------------------------------------------------------------------------------
class GPS_topic_publish:
def __init__(self, config_gps):
self.gps_sensor = GPS(config_gps) # 创建 GPS 实例
self.pub_gps = None # Publisher 尚未初始化
def gps_callback(self, gps_message):
# 获取带有噪声的状态
updated_state = self.gps_sensor.update(gps_message)
if updated_state is not None:
self.publish_state(updated_state)
def publish_state(self, state=None):
# 如果没有传入状态，则从 GPS 实例获取状态
if state is None:
state = self.gps_sensor.state()
# 创建并发布NavSatFix消息
gps_data_msg = NavSatFix()
gps_data_msg.header.stamp = rospy.Time.now()
gps_data_msg.header.frame_id = "gps_link"
gps_data_msg.latitude = state["latitude"]
gps_data_msg.longitude = state["longitude"]
gps_data_msg.altitude = state["altitude"]
# 设置状态
gps_data_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
gps_data_msg.status.service = NavSatStatus.SERVICE_GPS
# 发布消息到新的topic '/gps'
if self.pub_gps is not None:
self.pub_gps.publish(gps_data_msg)
else:
print("Publisher not initialized.")
def update(self, state: np.ndarray, dt: float):
pass
# --------------------------------------------------------------------------------
# Get Isaac Sim rostopic '/odom' data
# --------------------------------------------------------------------------------
class Get_Odom_Data:
def __init__(self):
self.converter = OdomToGPS()
self.gps = None
self.last_stamp = None
def odom_callback(self, data):
# 获取当前时间戳
current_stamp = data.header.stamp
# 如果是第一次回调，设置 last_stamp
if self.last_stamp is None:
self.last_stamp = current_stamp
return
# 计算时间差
dt = (current_stamp - self.last_stamp).to_sec()
self.last_stamp = current_stamp
record = {
'header': {
'seq': data.header.seq,
'stamp': {
'secs': data.header.stamp.secs,
'nsecs': data.header.stamp.nsecs
},
'frame_id': data.header.frame_id
},
'child_frame_id': data.child_frame_id,
'pose': {
'pose': {
'position': {
'x': data.pose.pose.position.x,
'y': data.pose.pose.position.y,
'z': data.pose.pose.position.z
},
'orientation': {
'x': data.pose.pose.orientation.x,
'y': data.pose.pose.orientation.y,
'z': data.pose.pose.orientation.z,
'w': data.pose.pose.orientation.w
}
},
'covariance': data.pose.covariance
},
'twist': {
'twist': {
'linear': {
'x': data.twist.twist.linear.x,
'y': data.twist.twist.linear.y,
'z': data.twist.twist.linear.z
},
'angular': {
'x': data.twist.twist.angular.x,
'y': data.twist.twist.angular.y,
'z': data.twist.twist.angular.z
}
},
'covariance': data.twist.covariance
}
}
if self.gps is not None:
self.converter.convert_odom_to_gps(record, self.gps)
self.gps.update(State(), dt)
# self.update_gps_data(gps_message)
def run(self):
rospy.init_node('odom_subscriber', anonymous=True)
rospy.Subscriber("/odom", Odometry, self.odom_callback)
rospy.spin()
# --------------------------------------------------------------------------------
# main
# --------------------------------------------------------------------------------
def main():
pub_gps = rospy.Publisher('/gps', NavSatFix, queue_size=10)
odom_handler = Get_Odom_Data() # 创建 Get_Odom_Data 实例
config_gps = _config_gps() # 获取 GPS 配置
gps = GPS(config_gps) # 创建 GPS 实例
gps.pub_gps = pub_gps # 确保 GPS 实例能够访问 pub_gps
odom_handler.gps = gps # 设置 Get_Odom_Data 的 gps 属性
# 设置 OdomToGPS 的 gps 属性以访问 Publisher
odom_handler.converter.gps = gps
odom_handler.run() # 启动 Get_Odom_Data 的 run 方法
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
passGPS噪声配置文件
isaac_sim_config_gps.yam
python
%YAML:1.0
#common parameters
update_rate: 250.0
# Define the GPS simulated/fixed values
GPS:
fix_type: 3
eph: 1.0
epv: 1.0
sattelites_visible: 10
gps_xy_random_walk: 2.0 # (m/s) / sqrt(hz)
gps_z_random_walk: 4.0 # (m/s) / sqrt(hz)
gps_xy_noise_density: 2.0e-4 # (m) / sqrt(hz)
gps_z_noise_density: 4.0e-4 # (m) / sqrt(hz)
gps_vxy_noise_density: 0.2 # (m/s) / sqrt(hz)
gps_vz_noise_density: 0.4 # (m/s) / sqrt(hz)
gps_correlation_time: 60 # s
# update_rate: 60 # Hz