devbox
Li_阴宅
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

px4飞控位置估计lpe移植到vs_block local position estimator

作者：Li_阴宅 | 2024-07-12 01:27:03

赞

踩

block local position estimator

本文主要内容

px4飞控的位置估计有两种方式，一是inav，二是lpe，用到的传感器用加速度计，磁场传感器，gps，超声，激光，气压，视觉，动作捕捉。但是动作捕捉在目前固件的inav中还不支持。

本文将进行移植lpe中使用加速度计，磁场传感器和GPS进行位置估计的部分。

本文的主要内容用：用到的px4固件中的文件，移植过程，移植完成后主函数内容，效果展示。

用到的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固件的写法重新写好放到程序源码文件夹下。

这是一个大概的流程，可能不一定要按照顺序来，但是每一条都至关重要。而且编译过程中还有其他更多的小问题，这就需要根据具体情况进行处理了。

移植完成后主函数内容

main.h中内容如下：
#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);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
main.cpp中内容如下：
/*
作者：高伟晋
时间：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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
BlockLocalPositionEstimator.h中内容：
#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
};

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186

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
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301

最终效果展示

lpe移植

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Li_阴宅/article/detail/812224
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号