位置解算之inav

以baro为例，函数计算如下：

z向位移和速度分别为：z_est[0](k) = z_est[0](k-1)+z_est[1](k-1)*dt +acc[2]*dt*dt/2z_est[1](k) = acc[2] * dt

wait_baro=falselocal_pos.z_valid = truelocal_pos.v_z_valid = true

position_estimator_inav_thread_main()

主题sensor_combined中的数据已更新

x_est[0] = 0.0f y_est[0] = 0.0f x_est[1] = gps.vel_n_m_s y_est[1] = gps.vel_e_m_s

否

是

lidar数据的有效性检查、数据更新与处理数据在主题distance_sensor中

函数inav_parameters_init()初始化各传感器的权重，存储在变量pos_inav_param_handles中

气压计的修正量 : corr_baro=corr_baro+w_z_gps_p * dt * corr_gps[2][0]

gps中数据精度是否满足要求

accel_updates++

如果lidar传感器无效，则使用baro对加速度z向修正，方法如下：

函数结束

由 att.q 得到旋转矩阵 R

while循环

R_gps= 前面第20组的旋转矩阵R_buf

利用函数inertial_filter_predict（）求解x y向的位置和速度，存储到变量x_est y_est中

by 蜗牛拉火车

气压计的零点漂移 : baro_offset(k) = baro_offset(k-1)+w_z_gps_p * dt * corr_gps[2][0]=baro_offset(0)+∑(w_z_gps_p * dt * corr_gps[2][0])

每隔0.01s发布一次数据

vision数据的有效性检查、数据更新与处理数据在主题vision_position_estimate中

eph epv在不超限的情况下： eph=eph*(1+dt) epv=epv+0.05*dt

同样方法，利用mocap vision对z向数据修正

处理如下

公告的新主题：vehicle_local_positionvehicle_global_position

计算GPS各参数的权重：w_xy_gps_p = 1.0 * w_gps_xyw_xy_gps_v = 2.0 * w_gps_xyw_z_gps_p=0.005 * w_gps_zw_z_gps_v = 0.0 * w_gps_z其中：前面乘的数字在params中配置

GPS的修正量=测量值-估计值：corr_gps[0][0] = gps_proj[0] - est_buf[0][0]corr_gps[1][0] = gps_proj[1] - est_buf[1][0]corr_gps[2][0] = local_pos.ref_alt - gps.alt - est_buf[2][0]corr_gps[0][1] = gps.vel_n_m_s - est_buf[0][1]corr_gps[1][1] = gps.vel_e_m_s - est_buf[1][1]corr_gps[2][1] = gps.vel_d_m_s - est_buf[2][1]

轮询失败

利用卡尔曼估计方法，结合lidar和GPS的测量值，对z向数据（z Vz az）更新

气压计的修正量 : corr_baro=baro_offset(k-1) - sensor.baro_alt_meter - z_est[0]其中：sensor.baro_alt_meter为气压计测量值 z_est[0]为上个循环的估计值 baro_offset(k-1) 为上一组循环的零点漂移

baro_updates++

函数 inertial_filter_correct()使用lidar或baro（lidar无效时才用baro）的修正量对z向位置和速度进行修正

所有样本求和样本数加一

函数inav_parameters_update()将pos_inav_param_handles中的配置参数复制给变量params

函数map_projection_project()由((gps.lon gps.lat)-ref)得到相对于初始位置的位置变化，存储到变量gps_proj中

z_est[0] = z_est[0] +0.5 * corr_baro * dtz_est[1] = z_est[1] +0.5*0.5* corr_baro * dt

依据配置的GPS的延迟补偿取前面第20组的估计值作为本次计算使用的估计值est_buf

采集的baro样本数小于200

几点说明

函数内部

加速度计的测量值减去偏移量(acc_bias(k-1))，再由机体坐标系转换到地理坐标系下，得到NED下的加速度acc

GPS数据的有效性检查、数据的更新与处理

sensor中加速度计的数据是否已更新

当x y向的测量源丢失时，缓慢减小x y向的速度：y_est[1] = y_est[1] - 0.5 * y_est[1] * dty_est[1] = y_est[1] - 0.5* y_est[1] * dt

轮询成功

corr_gps=0

local_pos.ref_lat = gps.latlocal_pos.ref_lon = gps.lonlocal_pos.ref_alt = gps.alt + z_est[0]

position_estimator_inav_main.cpp流程图

函数inertial_filter_predict（）

mocap数据的有效性检查、数据更新与处理数据在主题att_pos_mocap中

拷贝主题vehicle_attitude中的数据到变量att中，数据包括三轴角速度、四元数

利用GPS的修正量对z向数据修正：z_est[0] = z_est[0] +w_z_gps_p * corr_gps[2][0]* dtz_est[1] = z_est[1] +(w_z_gps_p)2 * corr_gps[2][0]* dt + w_z_gps_v * corr_gps[2][1] * dt

如果使用光流，降低GPS在xy向的权重：w_xy_gps_p = 0.1 * w_xy_gps_pw_xy_gps_v = 0.1 * w_xy_gps_v

extern \"C\" __EXPORT int position_estimator_inav_main指明函数入口

x_est[0] = gps_proj[0] y_est[0] = gps_proj[1] x_est[1] = gps.vel_n_m_s y_est[1] = gps.vel_e_m_s

拷贝主题vehicle_gps_position中的数据到变量gps

x_est y_est中数据的有效性检查

轮询超时

由baro修正得到的NED系下z向的加速度偏移量acc_bias[2]=acc_bias[2]+0.05*dt*( RT * accel_bias_corr[2] )

利用函数inertial_filter_correct()结合GPS vision mocap flow的修正量对x y向的数据进行修正

thread_should_exit=ture

打印各传感器的更新频率

光流数据的有效性检查、数据更新与处理数据在主题optical_flow中

accel_bias_corr[0]= -（corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p + corr_gps[0][1] * w_xy_gps_v）accel_bias_corr[1]= -（corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p + corr_gps[1][1] * w_xy_gps_v）accel_bias_corr[2]= -（corr_gps[2][0] * w_z_gps_p * w_z_gps_p + corr_gps[2][1] * w_z_gps_v）

函数map_projection_init()将gps测得的经纬度处理后(角度转弧度、sin、cos)作为初始经纬度存入到变量ref中

初始位置是否已初始化

订阅以下主题：parameter_update actuator_controls_0actuator_armed sensor_combinedvehicle_attitude vision_position_estimatevehicle_gps_position optical_flow att_pos_mocap distance_sensorvehicle_rates_setpoint

z_est中数据的有效性检查

position_estimator_inav_main()

计算由Vision mocap flow修正得到的加速度的偏移量，与上面GPS的处理方法类似，处理后累加即可

sensor中气压计的数据是否已更新

光流 GPS vision mocap lidar的超时检查

若主题中的数据更新，拷贝主题的数据到对应的变量当中：parameter_update ------ updateactuator_controls_0 ------ actuatoractuator_armed ------ armedparameter_update ------ update

accel_bias_corr[2] = - 0.5*0.5*corr_baro

gps_updates++

w_gps_xy=2/max(2，gps.eph)w_gps_z = 2/max(2，gps.epv)

拷贝主题sensor_combined中的数据到变量sensor中

计算循环周期dt

eph = min(eph，eph_vision，eph_flow，gps.eph，eph_mocap)epv = min(epv，gps.epv，epv_vision，epv_mocap)不明白为什么是取其中的最小值

attitude_updates++

配置轮询主题sensor_combined中数据的阻塞时间为1s

R_buf = R

配置轮询主题vehicle_attitude中数据的阻塞时间为20ms

wait_baro=false

1. 流程图中参数表示说明： ▲ xx[*][0] 代表的是位置 xx[*][1] 代表的是速度 ▲ xx(k)表示本次循环的值 xx(k-1)表示上次循环的值 ▲ 以w_开头的参数表示权重，在params中配置2. 部分参数所代表的含义▲ x_est，y_est，z_est都为2维向量，表示各向的位置和速度▲ acc：加速度 ▲ acc_bias：加速度的偏移量，用于对下一组采集的加速度修正▲ coor_gps coor_baro等：表示gps、baro等传感器的的修正 量，其求法为测量值减去上次循环的估计值▲ baro_offset：气压计的零点漂移(不太明白为什么每次要叠加 一个gps的修正，而且气压计的修正量corr_baro计算中也包 含了gps的修正量，希望懂的大神能够指点)3. 本函数的主要思路就是利用加速度积分求得位移和速度；利用 GPS、baro等传感器进行修正。其中，修正包含两个方面： (1) 对加速度计测得的加速度进行修正。加速度测量值减去上 次循环求得的加速度的偏移量acc_bias，得到本次计算中 积分使用的值。 (2) 对积分得到的位移和速度进行修正。各传感器的修正量 (coor_gps coor_baro等)乘以权重累加到位移和速度上4. 本流程图主要以GPS和气压计为例进行说明。 欢迎多批评指正！ 460864915@qq.com

由GPS修正得到的NED系下加速度的偏移量 acc_bias[*](k) =acc_bias[*](k-1)+0.05*dt*( R_gps(转置) * accel_bias_corr[*] )

将sensor_combined中的数据拷贝到变量sensor中，数据主要包括加速度计、陀螺仪、气压计、磁力计的测量数据

计算vision mocap等参数的权重

通过变量local_pos发布数据到主题vehicle_local_position中，包括 当前位置和速度(x_est，y_est，z_est)，计算精度（eph，epv），初始经纬度(ref_lat，ref_lon)(在刚开始读取GPS数值时赋值)，偏航角，各向位置和速度数据的有效性等

wait_baro=false(用于退出while循环)

该函数利用如下原理求解位移和速度：Δs = v*t + 0.5*a*t*tΔv = a*t

通过变量global_pos 发布数据到主题vehicle_global_position中，包括由位置变化计算得出的相对于初始位置的经纬度/高度变化，速度，计算精度（eph，epv），偏航角等