三.卡尔曼滤波器(EKF)开发实践之三: 基于三个传感器的海拔高度数据融合

本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍:

一.卡尔曼滤波器开发实践之一: 五大公式

二.卡尔曼滤波器开发实践之二:  一个简单的位置估计卡尔曼滤波器

三.卡尔曼滤波器(EKF)开发实践之三:  基于三个传感器的海拔高度数据融合   也就是本文

四.卡尔曼滤波器(EKF)开发实践之四:  ROS系统位姿估计包robot_pose_ekf详解

五.卡尔曼滤波器(EKF)开发实践之五:  编写自己的EKF替换robot_pose_ekf中EKF滤波器

六.卡尔曼滤波器(UKF)开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇

七.卡尔曼滤波器(UKF)开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇

        插几句话:
        前面第一节,介绍了下卡尔曼滤波器五大公式在实际开发实践中,从系统状态向量,,到,,再到针对系统测量值向量的,等,每个细节的具体的分析和设计方法. 再实际学习和实践中,边实践边回头学习,相信很快你也能掌握卡尔曼滤波器的使用方法;

         第2小节,举例了一个简单的卡尔曼滤波器例子,其实重要的是演示了卡尔曼滤波器从公式到代码的实践过程. 并且使用更简单直观的python语言,使用numpy支持库,对卡尔曼滤波器5大公式,做了一对一的实现.

        对python语言不熟悉的读者,也不要着急,后面所有例子我会采用C++语言编写.其核心代码就是TinyEKF类, 这个C++类也是第2节中python版EKF类的C++版. 

--------------------------------正文开始------------------------------------------

        话说飞行员驾着一架82年的老飞机从跑道上起飞,作为飞行员需要实时知道飞机的飞行海拔高度(Altitude).有三种传感器可以测量飞机的实时飞行海拔高度:

1.GPS设备获取海拔高度: x_gps;

2.重力陀螺仪(或惯性传感器)可以获取海拔高度:x_imu;

3. 还有一个气压计( Barometer)可以获取大气压值( barometric pressure): x_bp.

        这三种传感器设备受各种因素影响其获取测测量值并不是100%准确,时而偏高,时而偏低. 但其测量值都可以进行线性回归,线性计算,也就是说符合高斯分布(气压计是个例外,我们后面再细说).

        所以,需要有个算法,把这种三种不确定性传感器的数据融合为一个最优估计海拔高度值: x_altitude.  这里就用到卡尔曼滤波器, 多传感器数据融合,这也是卡尔曼滤波器的主要用途.

        下面我们开始做这个多传感器数据融合的系统分析:

        先了解下气压计的知识,我们需要的是海拔高速,但气压计给我们的是大气压值,所以我们需要做些功课:

海拔高度转大气压的公式：
P = P0 ×（1 - H / 44300)^5.256

大气压转海拔高度公式:

H = 44300 * ( 1 - (P/P0)^(1/5.256)  )

式中：H: 海拔高度，P0=大气压（0℃，101.325kPa）

                                                    大气压和海拔高度的曲线图 

        上图是根据公式在Excel中绘制的海拔-大气压关系曲线. 根据大气压和海拔高度的关系曲线可以看出, 从0到12000米海拔的整体看这是一种非线性关系,虽然在低于4000米高度下,两者关系近似线性关系. 但为了把我们的卡尔曼滤波器改进成扩展的卡尔曼滤波器,即EKF. 我们扔把低空区域大气压和海拔高度的关系认为是非线性的. 进而演示非线性关系线性化,和雅各比矩阵在EKF中用法.

一. 确定系统状态列向量和:

        如上分析,系统的要求把三种具有不确定性的传感器测量值融合成一个海拔高度最优估计值.可以确定和是一个1维列向量: X(x_altitude)   系统状态值个数: n = 1.  就最优海拔高度值.

二. 确定系统状态转移矩阵或预测矩阵

        很明显,没有测量值的参与,系统状态值是一种恒等关系,可的: F_k = [ 1 ]        (n=1)

        Note:  这里定义的系统状态方程,严格来讲是不准确的. 因为飞机起飞爬升阶段. 飞机的高度状态本身在随上升速度和时间动态变化.  而这里的状态方程定义为: X_k = X_k-1.  如果要考虑这个因素的话,假设飞机垂直方向爬升速度恒定为v. 那么系统方程为:   X_k = X_k-1 + v * Δt    即先根据系统状态方程预测高度

即: F_k = [  1 + v * Δt  ]           (n=1)

但基于两个原因我没有这么做:

1. 为了系统更简单,.因为本例子重点在三路数据融合和雅各比矩阵用法;
2. 如果考虑爬升速度,那么就需要更精细地设计我的三路传感器的模拟测量值, 比如模拟测量值需要和因爬升的海拔变化大致吻合. 而那不是本文重点.   有兴趣的读者可以自己修改.

三. 控制矩阵和控制向量: 

        因为系统没有输入控制,所以我们公式<1>中就不需要项.  

        五大公式的公式<1>有了.

四. 系统不确定性协方差矩阵: 和

        根据第一节中分析,可得,系统状态值个数n=1, 而且我们的系统状态变换很可靠,我们可以给P_k(1x1)一个较小的不确定性,即方差. P_k(1x1) = [ 0.1 ]

五. 系统噪声协方差矩阵:   

        设置一个很小的系统噪音. 同理有:  Q_k(1x1) = [ 1e-4 ]

六. 系统测量值向量:

        根系上面系统需求. 我们有三个传感器的测量值,测量值个数: m = 3   

所以有:  z_k(3x1)  = ( x_gps, x_bp, x_imu)

七. 三个传感器的噪音协方差矩阵: 

 传感器测量值个数m=3,所以有:

为什么这样设置? 这三个传感器的可信度,或精度,或测量噪音是不一样的.  具体实践中可以根据参考传感器手册获取测量噪音.  我们这里就主观的认为:

R_k(1,1)针对gps的测测量值,噪音小,可信度高,所以给出一个尽量小值;

R_k(2,2)针对气压计的测测量值,噪音最大,可信度最低,所以给出一个稍大值;

R_k(3,4)针对惯性传感器的测测量值,噪音介于前两个中间,可信度中等,所以给出一个中间值;

这个噪音协方差的取值,对最终融合出的最优海拔高度估计值会有影响. 噪音最小的传感器,最优估计值会跟接近该传感器的测量值,反之亦然.

八.系统状态值(nx1)转测量值(mx1)的转换矩阵:

        首先根据第一节分析,我们知道 将是一个mxn矩阵.

        对于GPS和惯性传感器IMU来说,其传感器测量值,和系统状态值,使用同样的尺度和单位度量,也不需要使用线性方程y=kx+b做变换,是一种y=x相等关系.  所有在H_k中对应的变换关系可以设置为1.

       但对于气压器来说就不同了.   气压计获取的测量值单位是kPa. 根据上面文章开头分析,从kPa到海拔高度需要一个气压到海拔高度的公式做变化. 直观的从其曲线如可以看出,这是一种非线性关系, 无法直接使用限于线性计算和高斯分布的卡尔曼滤波器进行直接融合. 

        这里就需要一个非线性关系线性化的过程, 有了此过程, 并将其设置进矩阵, 我们的就可以称为雅各比矩阵了. 我们的滤波器也升级为扩展的卡尔曼滤波器了,即EKF.

        具体怎么进行非线性关系的线性化,我在第一节,五大公式介绍中已经做了介绍. 这我们直接写做法.  根据大气压转海拔高度公式: H = f(p) = 44300 * ( 1 - (P/P0)^(1/5.256)  ),我们求其导函数不太方便. 但我们可以很轻松计算出其连续差分,即在曲线某一点的切线的斜率. 

         p为大气压测量值

得:

好了,至此,整个EKF的五大公式需要的 向量和矩阵我们都设置完毕.  开始贴代码:

代码一:   EKF的核心基类TinyEKF,功能同python版TinyEKF:   Tiny.h

#ifndef __TINY_EKF_INCLUDE__
#define __TINY_EKF_INCLUDE__

#include <bfl/wrappers/matrix/matrix_wrapper.h>

using namespace MatrixWrapper;

namespace ekf
{

    class TinyEKF
    {
    public:
        // constructor
        TinyEKF(int numOfState, int numOfMeasurement, float pVal, float qVal, float rVal);

        // destructor
        ~TinyEKF();

        void updateRk(SymmetricMatrix &newRk);

        void setAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix);
        void getAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix);

        void setInitState(ColumnVector& _x_k, SymmetricMatrix& p_current);

        void getPostCovariance(SymmetricMatrix &matrix);

        //输入当前测量值,  返回新的最优评估值
        void doStep(ColumnVector &z, ColumnVector &x_k);

        /*
        根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
        # (1). X_k = F_k * X_k-1 + B_k * u_k
        */
        virtual bool stateTransitionFunction(ColumnVector &x, ColumnVector &x_new, SymmetricMatrix &f_k)
        {
            return false;
        }

        /*
        预测值状态变换函数(即测量值转状态值得反函数),把预测值状态向量转换为与测量值向量同样的单位或度量,
        必要时需要非线性函数线性化(返回雅各比矩阵)
        方程(4). X^_k = X_k + K_k * (z_k - zz)     # zz = H_k * X_k
        */
        virtual bool stateToMeasurementTransitionFunction(ColumnVector &x, ColumnVector &zz_k, Matrix &h_k)
        {
            return false;
        }

    private:
        int numOfState;       //状态向量个数
        int numOfMeasurement; //测量值向量个数

        ColumnVector X_k;          //上一时刻(k-1)或当前时刻k的状态向量:  n个元素向量,或nx1矩阵
        SymmetricMatrix Q_k;       //各状态变量的预测噪声协方差矩阵nxn
        SymmetricMatrix P_current; //前一时刻(k-1)预测协方差矩阵: 预测P_k
        SymmetricMatrix P_result;  //最优P_k: 当前时刻最优估计协方差矩阵

        /*
        For update
    
        # 传感器测量值向量与预测值向量之间的非线性转换矩阵
        # mxn矩阵的元素为: 状态值和观测值之间非线性函数的一阶导数,或有限差分, 或连续差分的比值
        # m为测量值个数, n为状态量个数, 用处1: H_k(mxn) 乘 X(nx1) = ZZ_k(mx1)
        # H_k = np.eye(n)  # 返回的是一个二维的数组(m,n)，对角线的地方为1，其余的地方为0.

        # 传感器自身测量噪声带来的不确定性协方差矩阵
        # Set up covariance matrices for measurement noise
        */
        SymmetricMatrix R_k;

        //单位矩阵I, 这里当数字1使用.  P_k = P_k - K_k*H_k*P_k = (I - G_k*H_k)*P_k
        //Identity matrix(单位矩阵) will be useful later
        SymmetricMatrix Identity;
    };

}
#endif

 代码二:   EKF的核心积累:   Tiny.cpp

#include "TinyEKF.h"

using namespace std;

namespace ekf
{
    // constructor
    TinyEKF::TinyEKF(int _numOfState, int _numOfMeasurement, float pVal = 0.1, float qVal = 1e-4, float rVal = 0.1) : numOfState(_numOfState),
                                                                                                                      numOfMeasurement(_numOfMeasurement)
    {
        X_k.resize(_numOfState);
        X_k = 0;

        Q_k.resize(_numOfState, true);
        Q_k = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            Q_k(i, i) = 1;
        Q_k *= qVal;

        P_current.resize(_numOfState, true);
        P_current = 0;

        P_result.resize(_numOfState, true);
        P_result = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            P_result(i, i) = 1;
        P_result *= pVal;

        R_k.resize(_numOfMeasurement, true);
        R_k = 0;
        for (unsigned int i = 1; i <= _numOfMeasurement; i++)
            R_k(i, i) = 1;
        R_k *= rVal;

        Identity.resize(_numOfState, true);
        Identity = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            Identity(i, i) = 1;
    }

    // destructor
    TinyEKF::~TinyEKF()
    {
    }

    void TinyEKF::setInitState(ColumnVector& _x_k, SymmetricMatrix& _p_k)
    {
        for (unsigned int i = 1; i <= numOfState; i++)
            X_k(i) = _x_k(i);
        for (unsigned int i = 1; i <= 6; i++)
            for (unsigned int j = 1; j <= 6; j++)
                P_result(i, j) = _p_k(i, j);
    }

    void TinyEKF::updateRk(SymmetricMatrix &newRk)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                R_k(i, j) = newRk(i, j);
    }

    void TinyEKF::setAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix /*measurementCount * measurementCount*/)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                R_k(i, j) = noiseMatrix(i, j);
    }

    void TinyEKF::getAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix /*measurementCount * measurementCount*/)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                noiseMatrix(i, j) = R_k(i, j);
    }

    void TinyEKF::getPostCovariance(SymmetricMatrix &matrix)
    {
        for (unsigned int i = 1; i <= 6; i++)
            for (unsigned int j = 1; j <= 6; j++)
                matrix(i, j) = P_result(i, j);
    }

    void TinyEKF::doStep(ColumnVector &z, ColumnVector &x_k)
    {
        //Predict ----------------------------------------------------

        /*  根据系统运动学方程或几何学方程,更新预测状态*/
        /*# 预测状态方程
        # (1). X_k = F_k * X_k-1 + B_k * u_k
        # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
        # B_k: 系统外部控制矩阵 维数确保可以和前面F_k相乘
        # u_k: 系统外部控制向量,例如加速度,转向等.  维数确保可以和前面F_k相乘
        */
        ColumnVector x_k_new(numOfState);
        SymmetricMatrix F_k(numOfState);
        stateTransitionFunction(X_k, x_k_new, F_k);

        /*# 预测协方差矩阵
    # (2). P_k = F_k * P_k-1 * F_k^T + Q_k
    # Q_k: 各状态量对应的噪音协方差矩阵nxn
    */
        P_current = (SymmetricMatrix)(((SymmetricMatrix)(F_k * P_result)) * F_k.transpose() + Q_k); //P_k

        //Update -----------------------------------------------------
        /*# 预测值状态转测量值变换函数(即测量值转状态值得反函数)矩阵,把预测值状态向量转换为与测量值向量同样的单位或度量,
    # 必要时需要非线性函数线性化(返回雅各比矩阵)
    # 1. 一个m个元素的预估测量值向量: 经经雅各比矩阵 * 当前状态值向量得到的 "预估测量值向量"
    # H_k: 预测值状态转测量值变换函数的雅各比矩阵
    */
        ColumnVector zz_k(numOfMeasurement);
        Matrix H_k(numOfMeasurement, numOfState);
        stateToMeasurementTransitionFunction(x_k_new, zz_k, H_k);

        /* 卡尔曼增益: K_k
    # (3). K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
    # R_k: 传感器噪声(不确定性)协方差矩阵mxm
    */
        //Note:  这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对矩阵就逆矩阵（inverse()）时，运算结果不正确
        Matrix K_k;
        Matrix tmp1 = (Matrix)((Matrix)P_current * H_k.transpose());
        Matrix tmp2 = (Matrix)(H_k * (Matrix)P_current);
        Matrix tmp3 = (Matrix)(tmp2 * H_k.transpose());
#if true //分步计算
        Matrix tmp31 = (Matrix)(tmp3 + R_k);
        Matrix tmp4 = tmp31.inverse();
        K_k = tmp1 * (Matrix)tmp4;
#else
        K_k = (Matrix)(tmp1 * (Matrix)(((Matrix)(tmp3 + R_k)).inverse()));
#endif

        /*# 最终,最优预测状态向量值
    # z_k: 传感器读数或均值
    # (4). X^_k = X_k + K_k * (z_k - H_k * X_k)
    # self.x += np.dot(K_k, (np.array(z) - H_k.dot(self.x)))
    */

        /*# zz_k: 由状态转测量值函数stateToMeasurementTransitionFunction返回的值
    # (4). X^_k = X_k + K_k * (z_k - zz_k)    # Note:  此处的zz_k = H_k * X_k)
    */
        X_k = x_k_new + (K_k * (z - zz_k));

        /*# 最后,最优预测协方差矩阵
    # (5). P^_k = P_k - K_k * H_k * P_k = (I - K_k * H_k) * P_k
    */
        P_result = (SymmetricMatrix)(((SymmetricMatrix)(Identity - (K_k * H_k))) * P_current);

        x_k = X_k;
    }

}

 代码三. 针对此融合实例的EKF工作类: AltitudeDataFusion

AltitudeDataFusion.h

#ifndef __ALTITUDE_DATE_FUSION_4_TEST_INCLUDE__
#define __ALTITUDE_DATE_FUSION_4_TEST_INCLUDE__

#include "TinyEKF.h"

namespace ekf
{

class AltitudeDataFusion :  public TinyEKF
{
private:
    int stateCount = 1;
    int measurementCount = 1;
    int interval = 10;   // 预测更新时间间隔,单位 ms

    float last_state_altitude = 0.0; //上次状态值：海拔高度值   做非线性函数线性化,用于计算连续差分
    float last_measure_barometers = 0.0; //上次测量气压值
public:
    AltitudeDataFusion(int _numOfState, int _numOfMeasurement, float pVal, float qVal, float rVal,int _interval);
    ~AltitudeDataFusion(){};

    bool stateTransitionFunction(ColumnVector& x, ColumnVector& x_new, SymmetricMatrix& f_k);
    bool stateToMeasurementTransitionFunction(ColumnVector& x, ColumnVector& zz_k, Matrix& h_k);
};

}
#endif

 代码四. 针对此融合实例的EKF工作类: AltitudeDataFusion

AltitudeDataFusion.cpp

#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include "AltitudeDataFusion4Test.h"
#include <math.h>

using namespace std;

namespace ekf
{

    AltitudeDataFusion::AltitudeDataFusion(int _numOfState, int _numOfMeasurement, float pVal = 0.1, float qVal = 1e-4, float rVal = 0.1, int _interval = 10)
        : TinyEKF(_numOfState, _numOfMeasurement, pVal, qVal, rVal)
    {
        stateCount = _numOfState;
        measurementCount = _numOfMeasurement;
        interval = _interval; // 预测更新时间间隔,单位 ms

        last_state_altitude = 0.0;     //上次状态值： 海拔高度值   做非线性函数线性化,用于计算连续差分
        last_measure_barometers = 0.0; //上次测量气压值
    }

    /*
    根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
    # (1). X_k = F_k * X_k-1 + B_k * u_k
    */
    bool AltitudeDataFusion::stateTransitionFunction(ColumnVector &x, ColumnVector &x_new, SymmetricMatrix &f_k)
    {
        //状态转换矩阵设为I 因为k-1状态到K状态之间没有明确的转换关系, 我们假设为恒等关系
        SymmetricMatrix _f_k(stateCount); //矩阵nxn
        _f_k = 0;
        for (unsigned int i = 1; i <= stateCount; i++)
            _f_k(i, i) = 1;

        f_k = _f_k; // 返回一个单位矩阵.

        x_new = _f_k * x; // 这里直接返回了当前状态值得相同值

        //cout<<"stateTransitionFunction:   "<<"f_k="<<f_k<<",  x_new="<<x_new<<"\n";

        return true;
    }

    /*
    预测值状态变换函数(即测量值转状态值得反函数),把预测值状态向量转换为与测量值向量同样的单位或度量,
    必要时需要非线性函数线性化(返回雅各比矩阵)
    方程(4). X^_k = X_k + K_k * (z_k - zz)     # zz = H_k * X_k
    */
    bool AltitudeDataFusion::stateToMeasurementTransitionFunction(ColumnVector &x, ColumnVector &zz_k, Matrix &h_k)
    {
        /*
        大气压同海拔高度的关系：
        　　P=P0×（1-H/44300)^5.256
        计算高度公式为:
           H=44300*(1- (P/P0)^(1/5.256) )
        式中：H——海拔高度，P0=大气压（0℃，101.325kPa）
        */

        //基于大气压值: 把海拔高度估计值x,转换为大气压值,以便跟大气压的测量值做公式(4)中差运算
        float altitude = x[1]; //状态向量(1) only one
        float z_barometers = 101.325 * pow((1 - altitude / 44300), 5.256);
        float finite_difference = 1; // 默认变化率为1,  类似于 y = f(x) = x
        if (last_state_altitude != 0 && last_measure_barometers != 0 && (last_state_altitude - altitude) != 0)
            finite_difference = (z_barometers - last_measure_barometers) / (altitude - last_state_altitude);

        Matrix _h_k(measurementCount, stateCount); //测量值个数为3,  3行,1列
        _h_k = 0;
        _h_k(1, 1) = 1;
        _h_k(2, 1) = finite_difference;
        _h_k(3, 1) = 1;

        //返回经状态转换函数变换后的测量值zz_k: H_k(mxn) * X(nx1) = ZZ_k(mx1)
        zz_k = _h_k * x;
        h_k = _h_k;

        last_state_altitude = altitude;
        last_measure_barometers = z_barometers;

        return true;
    }

}

 代码五. 是主程序main.   代码中三个传感器测量数据,在此实例的python版本中利用程序模拟得出.也就是说,本代码使用的三个传感器测试数据都为程序计算出的符合线性计算和高斯分布的虚拟数据.

test_AltitudeDataFusion.cpp

#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include "AltitudeDataFusion4Test.h"

using namespace std;
using namespace ekf;

/*================================================================================================================
Note:  使用此test_AltitudeDataFusion请在安装好以来后，在linux下单独编译或放在Ros某软件包下编译。
并在CMakeList.txt文件中add_executable（）添加：
src/AltitudeDataFusion4Test.cpp
src/test_AltitudeDataFusion.cpp
//=================================================================================================================
*/

const int DATA_COUNT = 30;
float measurement_gps[DATA_COUNT] = {0, 104, 210, 290, 398, 499, 613, 694, 812, 909, 993, 1091, 1208, 1301, 1395, 1509, 1605, 1691, 1806, 1906, 1996, 2089, 2206, 2309, 2391, 2502, 2607, 2690, 2789, 2907};
float measurement_barometers_H[DATA_COUNT] = {0, 75, 155, 332, 419, 509, 567, 721, 834, 938, 993, 1101, 1204, 1274, 1438, 1504, 1564, 1742, 1811, 1934, 1961, 2117, 2192, 2259, 2403, 2464, 2556, 2708, 2782, 2920};
float measurement_barometers[DATA_COUNT] = {101.325, 100.4266099631849, 99.47544773789187, 97.39691042499965, 96.38822795303125, 95.3536816370964,
                                          94.69175182921913, 92.95225021091464, 91.69239628071, 90.5451380048492, 89.94313080200484, 88.77043762058652,
                                          87.66360020799254, 86.91777911154804, 85.19053188412288, 84.5033128758858, 83.88247028293578, 82.06233465092055,
                                          81.36543736747255, 80.13503058455179, 79.8669686844874, 78.33234584516421, 77.6030945763177, 76.95628843940422,
                                          75.58090547584266, 75.00431317586846, 74.14143986112667, 72.73344519196905, 72.05585485049728, 70.80589611085179};
float measurement_imu[DATA_COUNT] = {5, 109, 191, 276, 428, 490, 594, 678, 774, 925, 1000, 1086, 1173, 1321, 1386, 1488, 1613, 1702, 1806, 1878, 1977, 2094, 2181, 2277, 2375, 2505, 2611, 2697, 2790, 2872};
float stateXArray[DATA_COUNT];

/*
Note:  此案例仅仅依赖输入的模拟数据,产生模拟结果,无图形化界面显示.
需要查看图形曲线,请到目录下ekf_AltitudeDataFusion.xlsx查看

# 下面是某次运行本程序的输出结果
R = [[1.e-06 0.e+00 0.e+00]
    [0.e+00 1.e-02 0.e+00]
    [0.e+00 0.e+00 1.e-03]]

#基于 measurement_barometers:
stateXArray =[0.14953209537134615, 95.46584023034309, 200.29502957437685, 282.40686231986575, 388.65192735778544, 489.7344090308052,
               602.568686577119, 686.2450890345611, 801.189098995206, 900.1725172278325, 985.3377062894409, 1082.1607189648387,
               1197.2093484085533, 1292.5434152487187, 1386.3912160828295, 1498.6052924229382, 1596.2178206182107, 1683.2114311052546,
               1795.7809829820792, 1896.5728378370507, 1987.5526023065893, 2080.6023498384834, 2195.336977486254, 2299.249965958058,
               2383.218747691464, 2492.1417623942743, 2597.477309953391, 2682.3632538785755, 2780.1342709938444, 2896.1241138077853]
*/

int main(int argc, char **argv)
{
  /*
    # 模拟数据产生详见python版本:  ekf_AltitudeDataFusion.py
    */

  int state_count = 1;
  int measurement_count = 3;

  cout << "my_robot_pose_ekf              start\n";
  //Create a new Kalman filter for mouse tracking
  AltitudeDataFusion kalfilt(state_count, measurement_count, 0.01, 1e-4, 0.0001, 30);

  //更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差)
  SymmetricMatrix r_k(measurement_count); //矩阵3x3
  r_k = 0;
  r_k(1, 1) = 0.000001;  // GPS
  r_k(2, 2) = 0.01;      // 大气压测高
  r_k(3, 3) = 0.001;     // IMU 惯性传感器
  kalfilt.updateRk(r_k); // 更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差)
  cout << "my_robot_pose_ekf             R_k： " << r_k << "\n";

  ColumnVector estimateX;
  for (int index = 0; index < DATA_COUNT; index++)
  {
    ColumnVector measurementZ(measurement_count);
    measurementZ(1) = measurement_gps[index];
    measurementZ(2) = measurement_barometers[index];
    measurementZ(3) = measurement_imu[index];

    //输入当前鼠标位置测量值,  返回新的鼠标位置最优评估值
    kalfilt.doStep(measurementZ, estimateX);

    stateXArray[index] = estimateX(1);
  }
  cout << "my_robot_pose_ekf              end\n";

  cout << "Result: \nstateXArray=[";
  for (int index = 0; index < DATA_COUNT; index++)
    cout << stateXArray[index] << ",";
  cout << "]\n";

  return 0;
}

 最后.  是程序模式飞机飞行到3000米高度时, 运行获得最优估计值与三个测量值对比图(注:这里我把大气压测量值,转化成了海拔高度):

数据太密集看不清楚?,  我们取0--1000米之间数据:

 如图,  最优海拔高度估计值,更接近可信度高的GPS,  符合预期.