px4飞控的位置估计有两种方式，一是inav，二是lpe，用到的传感器用加速度计，磁场传感器，gps，超声，激光，气压，视觉，动作捕捉。但是动作捕捉在目前固件的inav中还不支持。
本文将进行移植lpe中使用加速度计，磁场传感器和GPS进行位置估计的部分。
本文的主要内容用：用到的px4固件中的文件，移植过程，移植完成后主函数内容，效果展示。
/Firmware/src/lib/geo下所有文件。
/Firmware/src/lib/mathlib下所有文件。
/Firmware/src/lib/matrix下所有文件。
/Firmware/src/modules/local_position_estimator/sensors/gps.cpp
/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
首先按照px4源码中文件的组织结构在vs工程下添加这些文件，然后修正各头文件之间的包含路径错
误。
把/Firmware/src/lib/geo/geo.c改成geo.cpp。
把/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp中类的基类去掉。然后改正因为没有基类而产生的部分成员变量没有定义错误，这些变量大部分是没有用到的，也可以直接删除。
去掉/Firmware/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp/Firmwar
e/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp中除GPS外其他传感器相关的成员变量和成员函数。
把代码中出现的__EXPORT全部去掉。
把部分未定义的宏在相应的.h文件中定义。
一部分包含文件需要c++11支持，需要vs2013以上版本。如果是vs2012以下版本，则一些头文件需要按照px4固件的写法重新写好放到程序源码文件夹下。
这是一个大概的流程，可能不一定要按照顺序来，但是每一条都至关重要。而且编译过程中还有其他更多的小问题，这就需要根据具体情况进行处理了。
#include <iostream>
#include<fstream>
#include "LocalPositionEstimator/BlockLocalPositionEstimator.h"
#include <windows.h>
#include "gyro/stdafx.h"
#include "gyro/Com.h"
#include "gyro/JY901.h"
using namespace std;
DWORD WINAPI mavThreadRead(LPVOID lpParam);
DWORD WINAPI mavThreadWrite(LPVOID lpParam);
/*
作者：高伟晋
时间：2017年7月1日
功能：进行陀螺仪数据和GPS数据的融合，得到位置坐标。
*/
#include"main.h"
//定时器和线程
HANDLE hTimer = NULL;
HANDLE hThreadRead = NULL;
HANDLE hThreadWrite = NULL;
LARGE_INTEGER liDueTime;
int totalNum = 0;
//串口
char chrBuffer[2000];
unsigned short usLength = 0;
unsigned long ulBaund = 9600, ulComNo = 4;
signed char cResult = 1;
//写文件
ofstream fout;
//中断循环控制
bool isrun = 0;
bool isinit = 0;
int main()
{
hTimer = CreateWaitableTimer(NULL, TRUE, "TestWaitableTimer");
if (hTimer)
{
printf("定时器开启\r\n");
}
liDueTime.QuadPart = -10000000; //1s
SetWaitableTimer(hTimer, &liDueTime, 0, NULL, NULL, 0);
hThreadRead = CreateThread(NULL, 0,
mavThreadRead, NULL, 0, NULL);
hThreadWrite = CreateThread(NULL, 0,
mavThreadWrite, NULL, 0, NULL);
while(1);
return 0;
}
DWORD WINAPI mavThreadRead(LPVOID lpParam)
{
while (cResult != 0)
{
cResult = OpenCOMDevice(ulComNo, ulBaund);
}
isinit = 1;
cout<<"陀螺仪初始化完成..."<<endl;
while (isrun)
{
usLength = CollectUARTData(ulComNo, chrBuffer);
if (usLength > 0)
{
JY901.CopeSerialData(chrBuffer, usLength);
}
Sleep(50);
}
return 0;
}
DWORD WINAPI mavThreadWrite(LPVOID lpParam)
{
while(!isinit);
//位置估计器
BlockLocalPositionEstimator lpe; //构造函数中进行初始化
time_t rawtime;
char filenametemp[15];
cout<<"请输入要保存数据的文件名：";
cin>>filenametemp;
strcat(filenametemp,".txt");
char* filename = filenametemp;
fout.open(filename); //不能有空格
isrun = 1;
while (isrun)
{
if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0)
{
printf("1秒定时器出错了\r\n");
}
else
{
time(&rawtime);
liDueTime.QuadPart = -50000; //0.005s -
SetWaitableTimer(hTimer, &liDueTime, 0, NULL, NULL, 0);
totalNum++;
//cout << totalNum << endl;
if(totalNum<1000)
{
lpe.update();
}
//cout << "at "<<ctime(&rawtime) << endl;
float x,y,z;
x = lpe.getx();
y = lpe.gety();
z = lpe.getz();
//system("cls");
//printf("Pos:%.3f %.3f %.3f\r\n", x, y, z);
if(totalNum<1000)
{
fout<<x<<" "<<y<<" "<<z<<endl;
}
else if(totalNum ==1000)
{
fout<<flush;
fout.close();
printf("文件输出完毕\r\n");
}
}
}
return 0;
}
#pragma once
#include "../geo/geo.h"
#include "../matrix/math.hpp"
using namespace matrix;
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
static const float BIAS_MAX = 1e-1f;
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
static const size_t N_DIST_SUBS = 4;
// for fault detection
// chi squared distribution, false alarm probability 0.0001
// see fault_table.py
// note skip 0 index so we can use degree of freedom as index
static const float BETA_TABLE[7] = {0,
8.82050518214f,
12.094592431f,
13.9876612368f,
16.0875642296f,
17.8797700658f,
19.6465647819f,
};
class BlockLocalPositionEstimator //: public control::SuperBlock
{
// dynamics:
//
// x(+) = A * x(-) + B * u(+)
// y_i = C_i*x
//
// kalman filter
//
// E[xx'] = P
// E[uu'] = W
// E[y_iy_i'] = R_i
//
// prediction
// x(+|-) = A*x(-|-) + B*u(+)
// P(+|-) = A*P(-|-)*A' + B*W*B'
//
// correction
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
//
// measurements:
//
// sonar: pz (measured d*cos(phi)*cos(theta))
//
// baro: pz
//
// flow: vx, vy (flow is in body x, y frame)
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: pz (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
//
// land (detects when landed)): pz (always measures agl = 0)
//
public:
// constants
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
enum {U_ax = 0, U_ay, U_az, n_u};
enum {Y_baro_z = 0, n_y_baro};
enum {Y_lidar_z = 0, n_y_lidar};
enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
enum {Y_sonar_z = 0, n_y_sonar};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
enum {
FUSE_GPS = 1 << 0,
FUSE_FLOW = 1 << 1,
FUSE_VIS_POS = 1 << 2,
FUSE_VIS_YAW = 1 << 3,
FUSE_LAND = 1 << 4,
FUSE_PUB_AGL_Z = 1 << 5,
FUSE_FLOW_GYRO_COMP = 1 << 6,
FUSE_BARO = 1 << 7,
};
enum sensor_t {
SENSOR_BARO = 1 << 0,
SENSOR_GPS = 1 << 1,
SENSOR_LIDAR = 1 << 2,
SENSOR_FLOW = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_VISION = 1 << 5,
SENSOR_MOCAP = 1 << 6,
SENSOR_LAND = 1 << 7,
};
enum estimate_t {
EST_XY = 1 << 0,
EST_Z = 1 << 1,
EST_TZ = 1 << 2,
};
// public methods
BlockLocalPositionEstimator();
void update();
inline float getx(){return _x(0);}
inline float gety(){return _x(1);}
inline float getz(){return _x(2);}
virtual ~BlockLocalPositionEstimator();
private:
// prevent copy and assignment
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
// methods
// ----------------------------
//
Vector<float, n_x> dynamics(
float t,
const Vector<float, n_x> &x,
const Vector<float, n_u> &u);
void initP();
void initSS();
void updateSSStates();
void updateSSParams();
// predict the next state
void predict();
void publishLocalPos();
// baro
int baroMeasure(Vector<float, n_y_baro> &y);
void baroCorrect();
void baroInit();
// gps
int gpsMeasure(Vector<double, n_y_gps> &y);
void gpsCorrect();
void gpsInit();
// misc
inline float agl() { return _x(X_tz) - _x(X_z); }
inline double getDt() { return 0.5; }
// map projection
struct map_projection_reference_s _map_ref;
// reference altitudes
float _altOrigin;
bool _altOriginInitialized;
float _baroAltOrigin;
float _gpsAltOrigin;
unsigned char _estimatorInitialized;
// state space
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Dcm<float> _R_att;
Vector3f _eul;
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
};
BlockLocalPositionEstimator.cpp中内容：
#include <iostream>
#include "BlockLocalPositionEstimator.h"
#include "../gyro/JY901.h"
#pragma warning(disable:4244)
using namespace std;
// required standard deviation of estimate for estimator to publish data
static const int EST_STDDEV_XY_VALID = 2.0; // 2.0 m
static const int EST_STDDEV_Z_VALID = 2.0; // 2.0 m
static const int EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
static const float P_MAX = 1.0e6f; // max allowed value in state covariance
static const float LAND_RATE = 10.0f; // rate of land detector correction
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// reference altitudes
_altOrigin(0),
_altOriginInitialized(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
_estimatorInitialized(0),
// kf matrices
_x(), _u(), _P(), _R_att(), _eul()
{
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
initSS();
_estimatorInitialized = true;
baroInit();
gpsInit();
cout << "lpe初始化完成..." << endl;
}
BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
{
}
BlockLocalPositionEstimator BlockLocalPositionEstimator::operator=(const BlockLocalPositionEstimator &)
{
return BlockLocalPositionEstimator();
}
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
float t,
const Vector<float, BlockLocalPositionEstimator::n_x> &x,
const Vector<float, BlockLocalPositionEstimator::n_u> &u)
{
return _A * x + _B * u;
}
void BlockLocalPositionEstimator::update()
{
bool gpsUpdated = 1;
bool baroUpdated = 0;
// do prediction
predict();
// sensor corrections/ initializations
if (gpsUpdated)
{
gpsCorrect();
}
if (baroUpdated)
{
baroCorrect();
}
publishLocalPos();
cout << "lpe updata!"<<endl;
}
void BlockLocalPositionEstimator::initP()
{
_P.setZero();
// initialize to twice valid condition
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
//=======================================================
//_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
//_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
use vxy thresh for vz init as well
//_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
//=======================================================
_P(X_vx, X_vx) = 2 * 0.3f * 0.3f;
_P(X_vy, X_vy) = 2 * 0.3f * 0.3f;
// use vxy thresh for vz init as well
_P(X_vz, X_vz) = 2 * 0.3f * 0.3f;
// initialize bias uncertainty to small values to keep them stable
_P(X_bx, X_bx) = 1e-6f;
_P(X_by, X_by) = 1e-6f;
_P(X_bz, X_bz) = 1e-6f;
_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
}
void BlockLocalPositionEstimator::initSS()
{
initP();
// dynamics matrix
_A.setZero();
// derivative of position is velocity
_A(X_x, X_vx) = 1;
_A(X_y, X_vy) = 1;
_A(X_z, X_vz) = 1;
// input matrix
_B.setZero();
_B(X_vx, U_ax) = 1;
_B(X_vy, U_ay) = 1;
_B(X_vz, U_az) = 1;
// update components that depend on current state
updateSSStates();
updateSSParams();
}
void BlockLocalPositionEstimator::updateSSStates()
{
// derivative of velocity is accelerometer acceleration
// (in input matrix) - bias (in body frame)
_A(X_vx, X_bx) = -_R_att(0, 0);
_A(X_vx, X_by) = -_R_att(0, 1);
_A(X_vx, X_bz) = -_R_att(0, 2);
_A(X_vy, X_bx) = -_R_att(1, 0);
_A(X_vy, X_by) = -_R_att(1, 1);
_A(X_vy, X_bz) = -_R_att(1, 2);
_A(X_vz, X_bx) = -_R_att(2, 0);
_A(X_vz, X_by) = -_R_att(2, 1);
_A(X_vz, X_bz) = -_R_att(2, 2);
}
void BlockLocalPositionEstimator::updateSSParams()
{
// input noise covariance matrix
_R.setZero();
//_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); LPE_ACC_XY
_R(U_ax, U_ax) = 0.012f * 0.012f;
//_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); LPE_ACC_XY
_R(U_ay, U_ay) = 0.012f * 0.012f;
//_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); LPE_ACC_Z
_R(U_az, U_az) = 0.02f * 0.02f;
// process noise power matrix
_Q.setZero();
//float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
float pn_p_sq = 0.1f * 0.1f;
//float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
float pn_v_sq = 0.1f * 0.1f;
_Q(X_x, X_x) = pn_p_sq;
_Q(X_y, X_y) = pn_p_sq;
_Q(X_z, X_z) = pn_p_sq;
_Q(X_vx, X_vx) = pn_v_sq;
_Q(X_vy, X_vy) = pn_v_sq;
_Q(X_vz, X_vz) = pn_v_sq;
// technically, the noise is in the body frame,
// but the components are all the same, so
// ignoring for now
//float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
float pn_b_sq = 0.001f * 0.001f;
_Q(X_bx, X_bx) = pn_b_sq;
_Q(X_by, X_by) = pn_b_sq;
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
/*float pn_t_noise_density =
_pn_t_noise_density.get() + (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));*/
float pn_t_noise_density = 0.001f + (1.0 / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
void BlockLocalPositionEstimator::predict()
{
// get acceleration
float acc[3],angle[3];
angle[0] = JY901.stcAngle.Angle[0] / 32768 * 180;
angle[1] = JY901.stcAngle.Angle[1] / 32768 * 180;
angle[2] = JY901.stcAngle.Angle[2] / 32768 * 180;
matrix::Euler<float> eul(angle[0],angle[1],angle[2]);
matrix::Quaternion<float> q(eul);
//q.from_euler(JY901.stcAngle.Angle[0] / 32768 * 180,JY901.stcAngle.Angle[1] / 32768 * 180,JY901.stcAngle.Angle[2] / 32768 * 180);
//from_euler(JY901.stcAngle.Angle[0] / 32768 * 180,JY901.stcAngle.Angle[1] / 32768 * 180,JY901.stcAngle.Angle[2] / 32768 * 180); //当前四元数 ================================
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
acc[0] = (float)JY901.stcAcc.a[0]/32768*16*9.81;
acc[1] = (float)JY901.stcAcc.a[1]/32768*16*9.81;
acc[2] = (float)JY901.stcAcc.a[2]/32768*16*9.81;
Vector3f a(acc[0], acc[1], acc[2]); //当前加速度 ================================
// note, bias is removed in dynamics function
_u = _R_att * a;
//_u(U_az) += 9.81f; // add g
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
don't integrate position if no valid xy data
//if (!(_estimatorInitialized & EST_XY))
//{
// dx(X_x) = 0;
// dx(X_vx) = 0;
// dx(X_y) = 0;
// dx(X_vy) = 0;
//}
don't integrate z if no valid z data
//if (!(_estimatorInitialized & EST_Z))
//{
// dx(X_z) = 0;
//}
don't integrate tz if no valid tz data
//if (!(_estimatorInitialized & EST_TZ))
//{
// dx(X_tz) = 0;
//}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX)
{
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// covariance propagation logic
for (int i = 0; i < n_x; i++)
{
if (_P(i, i) > P_MAX)
{
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (int j = 0; j < n_x; j++)
{
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_P += dP;
//_xLowPass.update(_x);
//_aglLowPass.update(agl());
}
void BlockLocalPositionEstimator::publishLocalPos()
{
//const Vector<float, n_x> &xLP = _xLowPass.getState();
//_pub_lpos.get().x = xLP(X_x); // north
//_pub_lpos.get().y = xLP(X_y); // east
//_pub_lpos.get().z = xLP(X_z); // down
//_pub_lpos.get().vx = xLP(X_vx); // north
//_pub_lpos.get().vy = xLP(X_vy); // east
//_pub_lpos.get().vz = xLP(X_vz); // down
}
