文章目录

前言

本文主要介绍PX4中基于卡尔曼滤波的位置解算（local_position_estimator），该模块通常与四元数互补滤波配合使用．
PX4固件版本：1.13.0
代码位置

一、组合导航系统模型

INS/GPS组合导航系统一般有两种信息融合方式:速度、位置组合(松耦合)和伪距、伪距率组合(紧耦合)，PX4采用前一种方式，这种方法的测量模型是线性的，可以用标准卡尔曼滤波器进行信息的融合。惯性导航系统(INS)以牛顿力学为基础，首先由姿态解算得到姿态矩阵，建立起导航坐标系。然后利用加速度计来测量无人机的实时加速度，将实时加速度通过姿态矩阵转换到导航坐标系中，经过一次积分运算得到实时速度，再一次积分运算得出实时位置.

二、代码解析

1. 主程序

void BlockLocalPositionEstimator: