ekf3用于估计位姿, 导航坐标系为NED(北东地) 数学推导参考PX4和APM导航解算过程推导-24维状态EKF matlab见InertialNav-Github ekf2代码介绍EKF2 AP_NavEKF3.cpp bool InitialiseFilter() 用于初始化滤波器, 滤波器可以有好几个core, 每个core或lane对应一种传感器和imu的状态估计, 初始化时EK3_PRIMARY可以在为某个core或lane. 但之后可能会在运行时被修改成其他的lane. Affinity允许不设置首要lane, 并用lane error存储不同传感器的差值, 使系统能够效果最好的作为状态估计. lane error随...

libraries/AP_VisualOdom/AP_VisualOdom.h 参数有相机的类别, 相机相对飞控的位移, 相机相对飞控摆放的方向(前后左右向下), 相机数据的缩放系数, 延迟, 速度\位置\yaw的噪声 float posErr = 0; float angErr = 0; if (!isnan(m.pose_covariance[0])) { posErr = cbrtf(sq(m.pose_covariance[0])+sq(m.pose_covariance[6])+sq(m.pose_covariance[11])); angErr =...

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp 主要定义了init(), handle_msp(), update(), start_calibration()等几个函数 init() 主要用于接收光流传感器的类型并定义backend, backend是libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h中定义的OpticalFlow_backend基类对象, 派生出了PX4Flow, MAVLINK等几个具体传感器类型的派生类, 每个派生中重载了这个类的update(), handle_msg()函数,...

apm坐标系 相机相比无人机中心: x正方向朝前 y正方向朝右 z正方向朝下 数据格式 GPS 经度，维度，高度的绝对值 gps获取到的数据的协方差(正常室外时协方差<1.5) topic: /gps/fix type: sensor_msgs/NavSatFix uint8 COVARIANCE_TYPE_UNKNOWN=0 uint8 COVARIANCE_TYPE_APPROXIMATED=1 uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2 uint8 COVARIANCE_TYPE_KNOWN=3 std_msgs/Header header uint32 seq time stamp...