位置解算之LPE流程图

检查状态_x中数据的有效性，只有有一个数据无效则将状态_x中的10个数据都置零

函数gpsCorrect()

初始化状态x的一步转移矩阵 _A(10*10)

从变量_sub_sensor (sensor_combined中取值)中读取加速度值a(3维向量)

状态向量 _x (10维)初始化为0

是

函数内部

lidar是否初始化

GPS是否有效

While()大循环

初始经纬度gpsLatOrigin gpsLonOrigin存入变量_map_ref中

lidarInit()

否

_x=_x+dx

函数updateSSStates()利用DCM更新_A

初始化GPS测量矩阵C(6*10)

求10个样本的平均值赋值gpsLat、gpsLon、gpsAlt作为纬度、经度、高度

几点说明： 1. predict()函数中在对_x进行预测时，用到的是连续型卡尔曼方法。连续型卡尔曼状态一步预测方程为： xdot = dx/dt = A * x + B * u故可以用四阶龙格库塔法进行微分方程求解。连续型卡尔曼估计的方差矩阵为： Pdot=dP/dt=A*P+P*AT-P*CT*R(逆)*C*P+Γ*Q*ΓT对于程序中采用的求解方法，因为没有采用量测更新，上式中的C R为零；认为输入也是一种过程噪声，得到如下公式： dP/dt=A*P+P*AT+B*R*BT+Q2. 函数gpsInit()和函数gpsCorrect()中用到的地图投影方法用到了地理坐标系和球面极坐标系的转换方法，请查阅相关资料。3. 由于对flow mocap等相关知识的缺乏，本流程图只以GPS为例进行说明。感谢 Fantasy、坏灵魂的好态度、牧羊少年等网友的指点！希望大家多批评指正，460864915@qq.com

是否为第一次 接受GPS数据

baro是否初始化

对_x做滤波处理后存储在_xLowPass._state中

配置轮询optical_flow、parameter_update、sensor_combined中数据的阻塞时间为100ms

函数gpsInit()

判断测量xy向的各传感器(GPS/vision/mocap/flow)是否初始化，只要有一个完成初始化且vxy_stddev_ok 为 true则置_validXY为ture

判断_P中关于地形高速tz的方差是否小于设定值(即tz的估计值是否可用)，是的话置_validTZ为ture

取_x中的高度和地形高度，(tz-z)做滤波处理后存储在_aglLowPass._state中，(tz-z)表示地平面以上高度

1.标识初始高度已赋值2. _altOrigin = _gpsAltOrigin该变量在baro中也会被赋值，gps和baro哪个先初始化，就取哪个的测量值

利用函数updateSSParams()和更新的主题数据更新 _Q和 _R

每隔50ms储存一次_x的值到变量_xDelay，最多能存10组

函数 update( )

将y中数据存储到变量_gpsStats中以计算采集样本数、对多次采样的样本求和、求均值等

函数publishLocalPos():利用变量_pub_lpos将数据发布到vehicle_local_position_s中，包括：(x y z Vx Vy Vz)、偏航角、初始经度纬度高度、估计的方差、数据有效性等

_P = _P - K * C * _P

函数 local_position_estimator_thread_main()

y=(px py alt Vn Ve Vd)

baroInit()

dx = K * r

(lat lon)角度转弧度得到(lat_rad lon_rad)

baro是否有效

对beta做卡方分布检验

检查一步转移矩阵_P中数据的有效性，无效则调用函数initP()重置_P

初始化控制输入u的噪声协方差矩阵 _R

S_I =inv(C * _P * CT + R)inv为求逆运算

●●sonar●flow●vision●mocap●land●●

上式得到的c是由球面极坐标系下的角度变化(弧度)，乘以地球半径R得到的就是位移变化

检查数据是否满足要求：卫星数≥6精度满足要求等

1

检查用于本次更新的数据是否有效

从sub_att (vehicle_attitude中取值)中读取四元数，计算得到欧拉角和旋转矩阵_R_att

thread_should_exit=ture

lidar是否有效

计算更新周期dt

初始高度_gpsAltOrigin = gpsAlt + z

2

判断_P中关于z向位置的方差是否小于设定值（即z的估计是否可用），是的话置z_stddev_ok为ture

利用变量_pub_innov将数据发布到ekf2_innovations_s中，包括：gpsCorrect()函数中的r、GPS的量测矩阵R

函数predict()

对dx做限制性处理

_validXY=ture且初始经纬度未设置，则将类构造函数中配置的经纬度设置为初始经纬度

函数updateParams()

将_P的左下部分直接对称到其右上部分，以确保_P的对称性

_x = _x + dx

lidarCorrect()

函数updateParams()不知道干啥用的

函数 local_position_estimator_main()

extern \"C\

判断_P中关于xy向速度的方差是否小于设定值（即Vx Vy的估计是否可用），是的话设置vxy_stddev_ok 为 true

local_position_estimator流程图

利用四阶龙格库塔法求解dx，方法如下（为了便于书写，省略_x _u _A _B前的下划线）

函数gpsMeasure(y)

将a利用旋转矩阵转换到地理坐标系下，得到控制信号_u

函数map_projection_project()

从变量_sub_gps(vehicle_gps_position中取值)中读取GPS相关数据，包括使用的卫星数，垂直/水平精度等

dP的限制性处理

函数publishEstimatorStatus():利用变量_pub_est_status将数据发布到estimator_status_s中，包括：状态_x、矩阵_P、传感器有效性标志位、超时标志位、位置测量的水平/垂直精度等

订阅以下主题：actuator_armed vehicle_land_detectedvehicle_attitude optical_flowsensor_combined parameter_updatemanual_control_setpoint vehicle_gps_position distance_sensorvision_position_estimate att_pos_mocap设置各主题的更新频率

K = _P * CT * S_I

(这块理解不知道对不对)while循环每执行几次(50ms)，变量_xDelay存储一次估计的状态_x的值，最大能存储10组。GPS依据延迟补偿时间，计算出应该使用哪组估计值作为本次更新的预测值x0

检查各传感器是否有效

dP=(_A*_P+_P*_AT+_B*_R*_BT+_Q)*dt

函数initSS()

在类BlockLocalPositionEstimator的定义和构造函数中做如下工作

alt = _gpsAltOrigin - alt

x_rad=x/R y_rad=y/R 得到由(x y)得到的角度(经纬度)变化，其中R为地球半径

函数updateSSParams()

函数initP()初始化状态协方差矩阵 _P(10*10)

baroCorrect()

利用该函数由经纬度变化((lat lon) - _map_ref)得到位移变化(px py)（处理方法如下）

r = y - C * x0

初始化输入u的驱动矩阵 _B(10*3)

beta = (rT * (S_I * r))

采集样本数大于10个

利用该函数将当前经纬度减去已经发生的位移(x y)得到初始经纬度gpsLatOrigin gpsLonOrigin(处理方法如下)

integrate==ture是否利用加速度计进行积分

函数map_projection_reproject()

控制向量 _u (3维)初始化为0

函数结束

by 蜗牛拉火车

利用变量_pub_gpos将数据发布到vehicle_global_position_s中，包括：当前经纬度、当前海拔高度、当前地形高度、(Vx Vy Vz)、偏航角、估计精度等

配置参数的设置

_u=_u+(0，0，9.81)

更新订阅主题的数据

将lat lon转换为角度表示得到初始经纬度，存储到gpsLatOrigin gpsLonOrigin中

3

初始GPS测量噪声的协方差矩阵R(6*6)

dx的限制性处理

_P=_P+dP

初始化过程噪声w的协方差矩阵_Q

判断气压计是否已初始化，完成初始化且z_stddev_ok=ture则置_validZ为ture

函数gpsMeasure(y_global)

标识GPS已初始化、标识GPS无错误、_gpsStats置零以便接受下一组样本数据

GPS是否初始化

当未解锁时检测测距传感器lidar和sonar