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

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

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

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

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

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

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

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

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

上一节我介绍了UKF的算法思路. 并提供了关于EKF实现的6个步骤公式.看公式总是枯燥的.下面我将结合一个我精心设计的模拟飞机起飞爬升的例子来完成一个完整UKF开发实践.

Note： 本例基于简化UKF滤波算法（加性噪声）模型实现

---------------------------------------------------案例系统模型介绍------------------------------start

出于简单和方便大家模拟。本例子采用和前面飞机起飞爬升的例子。
但又有一些不同(假设飞机保持固定偏航角pitch起飞爬升，使用了固定加速度：a=20米/s^2 和固定俯扬角：θ=30°。
数据融合系统开始工作起始速度： 70米/s。因为飞机达到起飞速度才离地起飞)：
1。在EKF实现的例子中，只是简单的根据三个传感器数据融合了飞机起飞爬升阶段海拔高度。
本例中将同时计算水平前进距离：x,海拔高度：h,以及沿俯扬角的前进速度：v，即：系统状态值X（x，h,v）；
2。因为飞机爬升成非线性，本例采用UKF来实现；

同样，飞机上安装有三个传感器:
1.GPS定位系统,可以读取一个带有噪声的前进位移和海拔高度值: d_gps 和 h_gps 单位: 米;
2.气压计(barometers), 可以读取一个带有噪声的受温度和高度影响的气压值: p_bp 单位: kPa;
3.另外还有一个速度计（Speedometer），获取一个带有噪音的速度值: v_speed 单位: 米.
这样，我们的测量值列向量共4个： z_k(d_gps,h_gps,p_bp,v_speed)

****************************************************************************
实际工作中往往是根据测得的温度和压强垂直分布去计算位势高度，因几何高度难以直接测量。
大气压同海拔高度的关系：
　　P=P0×（1-H/44300)^5.256
计算高度公式为:
H=44300*(1- (P/P0)^(1/5.256) )
式中：H——海拔高度，P0=大气压（0℃，101.325kPa）
**************************************************************************

一。系统状态和测量值分析
系统状态列向量X_k: (d, h, v) n=3
系统控制有： a = 20米/s^2，俯扬角：θ=30°
系统初始速度为： 70米/s。因为飞机达到这个速度才离地起飞， x 和 h值也从此时开始计算。
系统测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed) m=4
二。系统运动学方程
同KF公式（1）： X_k = F_k * X_k-1 + B_k * u_k

也可用运动状态转移函数f(x)代替,即直接写下面运动学公式.
根据初中物理运动学公式:
vt = v0 + a * Δt
st = s0 + v0 * Δt + 0.5 * a * (Δt)^2
考虑固定加速度：a=20米/s^2 和到固定俯仰角： θ=30°
得系统运动学状态转移模型为：
水平方向新位置x: x_k = x_k-1 + 0 + cos(30°)*v_k-1*Δt + 0.5*cos(30°) * a * (Δt)^2
垂直方向新位置h: h_k = 0 + h_k-1 + sin(30°)*v_k-1*Δt + 0.5*sin(30°) * a * (Δt)^2
沿固定俯仰角的速度v: v_k = 0 + 0 + v_k-1 + a * Δt
其： F_k:
| 1 0 cos(30°)*Δt | | 1 0 0.5*√3*Δt |
| 0 1 sin(30°)*Δt | = | 0 1 0.5*Δt |
| 0 0 1 | | 0 0 1 |
列向量B_k: (省略u_k，因为a固定值，所以它和B_k合并)
| 0.5*cos(30°) * 20 * (Δt)^2 | | 5√3 * (Δt)^2 |
| 0.5*sin(30°) * 20 * (Δt)^2 | = | 5 * (Δt)^2 |
| 20 * Δt | | 20 * Δt |

注: 加速度a和俯仰角(爬升角)θ,我已经直接用数字写入方程,.所以后面代码看不到a和θ

其他：
1。因为本例基于简化UKF滤波算法（加性噪声）模型实现。所有Q_k和R_k也是单独一个矩阵，
2。 UKF滤波算法中没有明确的状态值到测量值转移矩阵H_k, 所以不定义,用h(x)替换；
3。 UKF滤波算法中，对卡尔曼增益K 和系统不确定性协方差矩阵P_k的计算与 KF和EKF不同。不能直接套用卡尔曼滤波器五大公式；

---------------------------------------------------案例系统模型介绍-------------------------------end

Note: 源码说明

1.因为我是使用ROS环境,所以我是在ROS下创建的一个包实现的. 只是图方便使用了ROS的CMakelist.txt,编译出可执行文件运行. 尽快我的demo程序和ros没有任何关系.;

2.因为涉及到矩阵和向量的操作.我是用的BFL库的矩阵封装API. 也是图方便系统有. 因为各个语言的矩阵API不一样.说以实现上会有不同.(#include <bfl/wrappers/matrix/matrix_wrapper.h>)

3.因为代码有注解,搭配UKF公式,我就不详细介绍每个方法了.

一. UKF类头文件(局部):

内容不再详细介绍,我加了必要的注释. 学过UKF算法的都不是问题.

#define K (3) // 在UKF公式中： k为第二个尺度参数，通常设置为3或3-n。  n为状态向量维数，或状态值个数
#define ALPHA (1e-3)  //a的取值一般在[1e−4,1)区间内
#define BETA (0)     //如果状态变量是标量的话，则β=0最优ColumnVector X_k; //当前时刻k的先验估计状态向量:  n个元素列向量， 其实就是预测和更新过程的中间状态值
ColumnVector X_post; //当前时刻后验估计： 最优状态估计值： n个元素列向量SymmetricMatrix P_k; //当前时刻k的先验估计 预测协方差矩阵:  其实就是预测和更新过程的中间状态值
//UKF中P_post根据sigma points计算出来的， 故没有初始值
SymmetricMatrix P_post;    //最优P_k: 当前时刻最优估计协方差矩阵SymmetricMatrix F_k; //系统状态转移矩阵nxn。  此值依赖Δt，故是动态计算
ColumnVector B_k;    //系统状态控制向量（矩阵）nx1。  此值依赖Δt，故是动态计算SymmetricMatrix Q_k; //系统预测过程噪声协方差矩阵nxn//传感器自身测量噪声带来的不确定性协方差矩阵
SymmetricMatrix R_k;//Identity matrix(单位矩阵)： 单位矩阵I, 矩阵运算中当数字1使用
SymmetricMatrix Identity;
public:
UKF(int _stateCount, int _measurementCount);
~UKF();//根据使用的多个传感器本身的噪音尺度,更新传感器噪音协方差矩阵
void UpdateRK(SymmetricMatrix &r);//传入调用时间间隔Δt和多个传感器测量值.
void PredictAndUpdate(double deltaT, ColumnVector &z /*in*/);//获取最优估计值
void GetPostEstimate(ColumnVector &x_post /*out*/);//, SymmetricMatrix &p_post /*out*/);//为了模拟测试，我们需要利用飞机的运动学模型生成一些模拟的测量数据：测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4
void GenarateDemoMeasurementData();private:
void UpdateFK(double deltaT);
void UpdateBK(double deltaT);
void Step1_GenerateSigmaPoints(ColumnVector &x /*X_post*/, SymmetricMatrix &P /*P_post*/, Matrix &Xsig);
void Step2_PredictSigmaPointsByStateFunction(Matrix &Xsig, Matrix &Xsig_pred, double deltaT);
void Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k);
void Step4_PredictSigmaPointsByMeasurementFunction(Matrix &xsig_pred, Matrix &zsig_pred);
void Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k);
void Step6_UpdatePostState(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k,Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k,ColumnVector &z_k, ColumnVector &x_post, SymmetricMatrix &p_post);

二. 初始化

UKF::UKF(int _stateCount, int _measurementCount){stateCount = _stateCount;measurementCount = _measurementCount;lambda = std::pow(ALPHA, 2) * (stateCount + K) - stateCount; // 在UKF公式中： k为第二个尺度参数，通常设置为3或3-n。  n为状态向量维数，或状态值个数lambda_plue_n_x_sqrt = std::sqrt(lambda + stateCount);       //√(λ+n)   一个常用中间值sigmaPointsCount = 2 * stateCount + 1;                          // Sigma points countweightVector.resize(sigmaPointsCount);weightVector = 0;//初始化各sigma points的权值double weight_0 = lambda / (lambda + stateCount);//W_0 = λ / (n + λ) weightforVariance4PointOne = weight_0 + (1- std::pow(ALPHA, 2) + BETA); // λ / (n + λ)  + (1 - a^2 + β)weightVector(1) = weight_0; //W_0 = λ / (n + λ) for (int i = 2; i <= sigmaPointsCount; i++){   //其他2n个sigma points‘ weightsdouble weight = 0.5 / (stateCount + lambda); //W_i = 1 / 2(n + λ) i = 1,2, 3, ..., nweightVector(i) = weight;}
#if (DEBUG)cout << "UKF::UKF====weightVector: " << weightVector << endl;
#endifX_k.resize(stateCount);X_k = 0.0;//UKF中P_k根据sigma points计算出来的,是个先验估计值， 故没有初始值P_k.resize(stateCount);P_k = 0.0;//最优估计值,或上一时刻最优估计值. 如分析描述，  系统初始速度为： 70米/s。 因为飞机达到这个速度才离地起飞， x 和 h值也从此时开始计算。X_post.resize(stateCount);X_post(1) = 0.0; // dX_post(2) = 0.0; // hX_post(3) = 70.0;// v//最优协方差矩阵,或上一时刻最优协方差矩阵P_post.resize(stateCount);P_post = 0.0;for (unsigned int i = 1; i <= stateCount; i++)P_post(i, i) = 1;P_post *= 1;//默认协方差，//F_k依赖Δt，后面动态计算F_k(1, 3)和F_k(2, 3)F_k.resize(stateCount);F_k = 0.0;for (unsigned int i = 1; i <= stateCount; i++)F_k(i, i) = 1;//B_k依赖Δt，后面动态计算B_k.resize(stateCount);B_k = 0.0;Q_k.resize(stateCount);Q_k = 0.0;for (unsigned int i = 1; i <= stateCount; i++)Q_k(i, i) = 1;Q_k *= 1e-4; //预测过程噪音值。 这个值填一个较小的值即可R_k.resize(measurementCount);R_k = 0.0;R_k(1, 1) = 0.0000001; // GPS噪音 -> dR_k(2, 2) = 0.0000001; // GPS噪音 -> hR_k(3, 3) = 0.00001;     // 气压计噪音噪音 -> hR_k(4, 4) = 0.00000001; // 速度计噪音噪音  -> v//单位矩阵，可能没用Identity.resize(stateCount);Identity = 0.0;for (unsigned int i = 1; i <= stateCount; i++)Identity(i, i) = 1;}

三. 提供若干个public方法

UpdateRK用于外部调节四个传感器的噪音;

UpdateFK和UpdateBK用于根据Δt动态调整R_k和B_k矩阵;

GetPostEstimate用于每次预测/更新完毕时,从外部获取当前最优估计值.

    void UKF::UpdateRK(SymmetricMatrix &r){R_k = 0.0;R_k(1, 1) = r(1, 1); // GPS噪音 -> dR_k(2, 2) = r(2, 2); // GPS噪音 -> hR_k(3, 3) = r(3, 3);     // 气压计噪音噪音 -> hR_k(4, 4) = r(4, 4); // 速度计噪音噪音  -> v}//仅动态计算F_k(1, 3)和F_k(2, 3)，其他保持不变void UKF::UpdateFK(double deltaT){static double _SquareRootValue = 0.5 * std::sqrt(3); //cos(30°)= √3 / 2//值的来历， 请参见文件头的状态分析F_k(1, 3) = _SquareRootValue * deltaT; //cos(30°)*ΔtF_k(2, 3) = 0.5 * deltaT;              //sin(30°)*Δt}//动态计算B_kvoid UKF::UpdateBK(double deltaT){static double _SquareRootValue2 = 5 * std::sqrt(3); // 1/2 * cos(30°) * 20 = 1/2 * (√3 / 2) * 20//值的来历， 请参见文件头的状态分析B_k(1) = _SquareRootValue2 * std::pow(deltaT, 2); //0.5*cos(30°) * 20 * (Δt)^2B_k(2) = 5 * std::pow(deltaT, 2);                 //0.5*sin(30°) * 20 * (Δt)^2B_k(3) = 20 * std::pow(deltaT, 2);                //20 * Δt}void UKF::GetPostEstimate(ColumnVector &x_post /*out*/) //, SymmetricMatrix &p_post /*out*/){x_post = X_post;//p_post = P_post;}

四. 第(1)步,根据初始或上次最优估计值X_post生成sigma point 3x7矩阵Xsig n=3 2n+1=7

代码如下(不同语言,不同矩阵api代码会不同, 我使用的矩阵api比较弱. ):

void UKF::Step1_GenerateSigmaPoints(ColumnVector& x /*X_post*/, SymmetricMatrix& p /*P_post*/, Matrix &Xsig)
{//calculate square root of Pfor (unsigned int row = 1; row <= stateCount; row++){for (unsigned int col = 1; col <= stateCount; col++){p(row, col) = std::sqrt(p(row, col));//Note: 这里前一行sqrt时,可能出现-nan情况,   判断并设置为0.0if( isnan( p(row, col)) )p(row, col) = 0.0;}//借用一下这个for循环，做点工作Xsig(row, 1) = x(row); //Xsig的第1列为x列向量本身}//设置其他2n个sigma点for (unsigned int row = 1; row <= stateCount; row++){for (unsigned int col = 1; col <= stateCount; col++){Xsig(row, col + 1) = x(row) + lambda_plue_n_x_sqrt * p(row, col); //Xsig的第2--n+1列Xsig(row, col + 1 + stateCount) = x(row) - lambda_plue_n_x_sqrt * p(row, col); //Xsig的第n+2--2n+1列}}
}

五. 第(2)步对生成的sigma points 矩阵Xsig按照状态转移方程进行预测,得到矩阵Xsig_pred:

或

void UKF::Step2_PredictSigmaPointsByStateFunction(Matrix &Xsig, Matrix &Xsig_pred, double deltaT)
{//按照系统状态方程（X_k = F_k * X_k-1 + B_k * u_k），更新2n+1个 sigma pointsfor (unsigned int col = 1; col <= sigmaPointsCount; col++){ColumnVector XsigCol_ = Xsig.columnCopy(col);ColumnVector XsigCol_2 = F_k * XsigCol_ + B_k; //类似KF/EKF公式（1）： X_k = F_k * X_k-1 + B_k * u_kfor (unsigned int row = 1; row <= stateCount; row++){Xsig_pred(row, col) = XsigCol_2(row);}}
}

六. 第(3)步根据矩阵Xsig_pred,按照预先设置的权值列向量W,计算其先验估计值x_k和先验协方差矩阵P_k

当i=0时:

void UKF::Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k)
{//计算先验估计值： state meanx_k = (ColumnVector)(xsig_pred * weightVector); //x_k(nx1) = xsig_pred(nx(2n+1)) * weightVector((2n+1)x1)//计算先验协方差矩阵p_k = 0.0;for (int col = 1; col <= sigmaPointsCount; col++){//各个sigma points相对于先验估计状态值x_k的距离ColumnVector x_diff = xsig_pred.columnCopy(col) - x_k; //x_diff(nx1)   x_k(nx1)//定义一个(stateCount, 1)矩阵，下面p_k的计算必须用矩阵，ColumnVector不可用Matrix x_diffMatrix(stateCount, 1);                    for (int row = 1; row <= stateCount; row++)x_diffMatrix(row, 1) = x_diff(row);//按照公式计算先验协方差矩阵if( col == 1 )  //对于第一个sigma points点,求其方差时,使用weightforVariance4PointOnep_k = p_k + (SymmetricMatrix)((Matrix)(weightforVariance4PointOne * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)elsep_k = p_k + (SymmetricMatrix)((Matrix)(weightVector(col/*row*/) * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)}
}

七.第(4)步根据第(2)步的Xsig_pred, 经状态值转测量值函数f(x),转换成4行7列sigma预测测量值矩阵Zsig_pred: 注意:这里的行数为测量值向量维数m=4

void UKF::Step4_PredictSigmaPointsByMeasurementFunction(Matrix &xsig_pred, Matrix &zsig_pred)
{//transform 2n+1 sigma points into measurement space//z_k(d_gps,h_gps,p_bp,v_speed)for (int col = 1; col <= sigmaPointsCount; col++){ // extract values for better readibilitydouble d = xsig_pred(1, col);double h = xsig_pred(2, col);double v = xsig_pred(3, col);// measurement modelzsig_pred(1, col) = d; //d_gps   相等关系zsig_pred(2, col) = h; //h_gps   相等关系zsig_pred(3, col) = 101.325 * std::pow((1 - h / 44300), 5.256); //p_bp  <--  海拔高度转大气压  非线性关系zsig_pred(4, col) = v;//v_speed  相等关系}
}

八. 第(5)步. 根据这含有4行7列的Zsig_pred,按照预先设置的权值列向量W,计算其估计值向量zz_k和协方差矩阵ZP_k:

void UKF::Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k)
{//计算测量值均值： state meanzz_k = (ColumnVector)(zsig_pred * weightVector); //zz_k(mx1) = zsig_pred(mx(2n+1)) * weightVector((2n+1)x1)//计算测量值协方差矩阵zzp_k = 0.0;for (int col = 1; col <= sigmaPointsCount; col++){//各个sigma points相对于测量值zz_k的距离ColumnVector z_diff = zsig_pred.columnCopy(col) - zz_k; //z_diff(mx1)   z_k(mx1)//定义一个(measurementCount, 1)矩阵，下面zzp_k的计算必须用矩阵，ColumnVector不可用Matrix z_diffMatrix(measurementCount, 1);               for (int row = 1; row <= measurementCount; row++)z_diffMatrix(row, 1) = z_diff(row);//按照公式测量值协方差矩阵//Note:  zzp_k 这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对后面第6步矩阵求逆矩阵（inverse()）时，运算结果不正确zzp_k = zzp_k + ((Matrix)(weightVector(col /*row*/) * z_diffMatrix) * z_diffMatrix.transpose()) + (Matrix)R_k; //weightVector(i) * z_diff(mx1) * z_diff(1xm)}
}

九. 第(6)步结合最新测量值z_k,计算最优估计值x_post和最优协方差矩阵P_post:

void UKF::Step6_UpdatePostState(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k,Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k,ColumnVector &z_k, ColumnVector &x_post, SymmetricMatrix &p_post)
{//创建 cross correlation 矩阵： XZ（nxm）Matrix xzCross = Matrix(stateCount, measurementCount);xzCross = 0.0;for (int col = 1; col <= sigmaPointsCount; col++) //2n+1 个 simga points{//系统状态值向量：X state differenceColumnVector x_diff = xsig_pred.columnCopy(col) - x_k;//定义一个(stateCount, 1)矩阵，下面XZ的计算必须用矩阵，ColumnVector不可用Matrix x_diffMatrix(stateCount, 1);for (int row = 1; row <= stateCount; row++)x_diffMatrix(row, 1) = x_diff(row);//测量值向量：Z differenceColumnVector z_diff = zsig_pred.columnCopy(col) - zz_k;//定义一个(measurementCount, 1)矩阵，下面XZ的计算必须用矩阵，ColumnVector不可用Matrix z_diffMatrix(measurementCount, 1);for (int row = 1; row <= measurementCount; row++)z_diffMatrix(row, 1) = z_diff(row);xzCross = xzCross + (Matrix)(weightVector(col /*row*/) * x_diffMatrix) * z_diffMatrix.transpose();}//卡尔曼增益（gain）: (nxm)//Note:  zzp_k 这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对矩阵就逆矩阵（inverse()）时，运算结果不正确Matrix K_gain = xzCross * zzp_k.inverse(); //XZ(nxm) * zzp_k(mxm) = (nxm)//传感器测量值 - 预测测量值： Z_diffColumnVector Z_diff = z_k - zz_k;//update state mean and covariance matrixx_post = x_k + K_gain * Z_diff;p_post = p_k - (SymmetricMatrix)(K_gain * (Matrix)zzp_k * K_gain.transpose()); // (nxm) * (m*m)  * (m*n)
}

十. 执行预测/更新

void UKF::PredictAndUpdate(double deltaT, ColumnVector &z /*in*/)
{UpdateFK(deltaT);UpdateBK(deltaT);//create sigma point matrixMatrix Xsig = Matrix(stateCount, sigmaPointsCount);//Step 1Step1_GenerateSigmaPoints(X_post, P_post, Xsig);//create predict sigma point matrixMatrix Xsig_pred = Matrix(stateCount, sigmaPointsCount);//Step 2Step2_PredictSigmaPointsByStateFunction(Xsig, Xsig_pred, deltaT);//Step 3Step3_CalculateMeanAndCovarianceByWeight(Xsig_pred, X_k, P_k);//Step 4Matrix Zsig_pred = Matrix(measurementCount, sigmaPointsCount);Step4_PredictSigmaPointsByMeasurementFunction(Xsig_pred, Zsig_pred);//Step 5ColumnVector ZZ_k(measurementCount);ZZ_k = 0.0;Matrix ZZP_k(measurementCount, measurementCount);ZZP_k = 0.0;Step5_CalculateMeasurementMeanAndCovarianceByWeight(Zsig_pred, ZZ_k, ZZP_k);//Step 6Step6_UpdatePostState(Xsig_pred, X_k, P_k,Zsig_pred, ZZ_k, ZZP_k,z, X_post, P_post);
}

十一. 测试运行

int main(int argc, char **argv)
{   //ColumnVector stateX(3); //3个状态值 ColumnVector measurementZ(4); //4个测量值float deltaT = 0.5;//sUKF ukf(3/*状态值个数*/,4/*测量值个数*/);//为了模拟测试，我们需要利用飞机的运动学模型生成一些模拟的测量数据,同时模拟传感器误差：测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4//ukf.GenarateDemoMeasurementData();double d_measurement[60]= {45.476,92.282,101.418,175.885,205.681,248.808,316.264,374.051,460.168,506.615,586.392,689.5,764.937,849.705,939.803,1054.23,1166.99,1234.08,1369.49,1458.24,1584.32,1706.73,1828.47,2000.54,2126.94,2236.67,2413.73,2573.11,2698.83,2872.88,3030.26,3213.97,3349.01,3516.38,3729.08,3869.11,4082.47,4250.17,4481.19,4650.54,4911.22,5091.23,5292.57,5542.24,5765.24,5997.58,6212.24,6451.23,6668.55,6925.2,7203.19,7453.5,7684.14,7941.11,8230.42,8507.05,8758.01,9027.31,9336.93,9597.88};double h_measurement[60]= {34.75,65,62.75,71,112.75,142,203.75,245,240.75,284,346.75,397,408.75,470,563.75,615,653.75,703,798.75,864,936.75,966,1044.75,1158,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2043,2120.75,2271,2387.75,2475,2560.75,2721,2790.75,2960,3059.75,3216,3293.75,3439,3600.75,3745,3845.75,4021,4139.75,4298,4466.75,4617,4740.75,4929,5028.75,5236,5397.75,5546};//double h_measurement_barometers[60]= {0,0,62.75,71,112.75,142,133.75,165,240.75,284,310.75,353,408.75,470,493.75,555,653.75,703,738.75,806,870.75,966,1044.75,1092,1212.75,1290,1362.75,1458,1538.75,1621,1734.75,1829,1931.75,2007,2120.75,2199,2299.75,2435,2560.75,2649,2790.75,2890,3059.75,3134,3293.75,3439,3536.75,3665,3845.75,3949,4139.75,4252,4380.75,4533,4740.75,4841,5028.75,5174,5339.75,5546};double bp_measurement_barometers[60]= {101.325,101.325,100.573,100.474,99.9769,99.6295,99.7274,99.3571,98.464,97.9571,97.6446,97.1527,96.5066,95.8009,95.5283,94.8284,93.7087,93.1542,92.7534,92.0033,91.2857,90.2383,89.3797,88.8677,87.5701,86.748,85.9796,84.9818,84.1433,83.2961,82.1358,81.1844,80.1574,79.4119,78.2958,77.5353,76.565,75.278,74.0971,73.2774,71.9761,71.0761,69.558,68.9023,67.5085,66.2611,65.4322,64.3574,62.8669,62.0282,60.5024,59.6188,58.6183,57.4527,55.8926,55.1521,53.7868,52.7493,51.5854,50.1661};double v_measurement[60]= {79,90,99,111,121,127,139,150,157,171,182,188,201,207,219,227,237,247,257,268,278,292,300,311,322,331,338,348,359,372,379,392,401,412,418,432,440,447,459,472,478,491,501,507,521,531,539,549,558,567,579,591,600,610,622,632,642,648,657,672};//保存结果最优估计值X_post数组ColumnVector statePostEstimateArray[60];//我根据生成模拟测量值数据时,设置的噪音,大致设置下各传感器噪音的方差(协方差).  //实际项目中应从传感器说明手册或厂家获取.SymmetricMatrix R(4); R = 0.0;R(1, 1) = 0.0000001; // GPS噪音方差(协方差) -> dR(2, 2) = 0.0000001; // GPS噪音方差(协方差) -> hR(3, 3) = 0.00001;     // 气压计噪音噪音方差(协方差) -> hR(4, 4) = 0.00000001; // 速度计噪音噪音方差(协方差)  -> vukf.UpdateRK(R);//更新下传感器噪音for( int i = 0; i < 60; i++ ) // 模拟预测和更新周期是2， 即每秒2执行2次。  30秒执行60次.  deltaT = 0.5s,当然这里不用真去delay(500){measurementZ(1) = d_measurement[i];measurementZ(2) = h_measurement[i];measurementZ(3) = bp_measurement_barometers[i];measurementZ(4) = v_measurement[i];ukf.PredictAndUpdate(deltaT, measurementZ);ukf.GetPostEstimate(statePostEstimateArray[i] /*out*/);}/*打印结果  略*/return 0;
}

十二: 运行结果放在excel,查看测量值和最优估计曲线图

1.速度计测量计和最优估计速度值曲线(因为设置速度计测量噪音很小,所有两者几乎重合)

2.GPS测量高度值, 气压计测量高度值,和最优估计高度值曲线图.

可见,由于气压计测量的噪音误差较大, 三者之间的曲线由没有交错现象

3.GPS测量前进位置和最优估计位移曲线

因为GPS测测量噪音很小, 所有两者也是几乎重合

有不足之处欢迎留言指正,共同进步,谢谢!

另外别忘了点个赞哦.

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

六.卡尔曼滤波器开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
一. 卡尔曼滤波器开发实践之一: 五大公式详解
既然标题名称是开发实践,本系列文章将主要介绍如何在工程实践中使用卡尔曼滤波器,至于卡尔曼滤波器的五大公式如何推导而来,网上有很多大拿们写的都很精彩,这里不再叙述.可以参考了下面两篇博文: 1. 卡尔曼 ...
Android AR开发实践之七：OpenGLES相机预览背景绘制源码详解
Android AR开发实践之七:OpenGLES相机预览背景绘制源码详解目录 Android AR开发实践之七:OpenGLES相机预览背景绘制源码详解一.OpenGL ES渲染管线 1.基本处 ...
四.卡尔曼滤波器(EKF)开发实践之四: ROS系统位姿估计包robot_pose_ekf详解
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
五.卡尔曼滤波器(EKF)开发实践之五: 编写自己的EKF替换robot_pose_ekf中EKF滤波器
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
20189200余超 2018-2019-2 移动平台应用开发实践第七周作业
20189200余超 2018-2019-2 移动平台应用开发实践第七周作业布局在这一节中首先学习了java的页面布局,在此基础之上来进行了编程. 图片如下: 代码如下: *** 使用代码进行登录 ...
python web开发项目源码_Python + Flask 项目开发实践系列七
对于 Python + Flask 这种灵活的web开发框架,在前面的六个系列文章中详细的进行了说明,主要讲到了页面的首页加载时的页面渲染,增加功能,删除功能,修改功能,查询功能,查询详情功能等一些页 ...
COM组件开发实践（八）---多线程ActiveX控件和自动调整ActiveX控件大小(下)
源代码下载:MyActiveX20081229.rar 声明:本文代码基于CodeProject的文章<A Complete ActiveX Web Control Tutorial>修改 ...
《COM组件开发实践》系列文章
COM组件开发系列链接: 1,COM组件开发实践(一) 2,COM组件开发实践(二) 3,COM组件开发实践(三) 4,COM组件开发实践(四)---From C++ to COM :Part 1 5 ...

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

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

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

最新文章

热门文章