本系列文章主要介绍了如何在工程实践中使用卡尔曼滤波器。
二.卡尔曼滤波器开发实践二： 一个简单的位置估计卡尔曼滤波器
传感器的海拔高度数据融合">三.卡尔曼滤波器(EKF)开发实践三： 基于三个传感器的海拔高度数据集成
四.卡尔曼滤波器(EKF)开发实践四： ROS系统位置估计包robot_pose_ekf详解
五.卡尔曼滤波器(EKF)开发实践五： 编写自己的EKF替换robot_pose_ekf中EKF滤波器
六.卡尔曼滤波器(UKF)开发实践六： 卡尔曼滤波器无损(UKF)高级-白话讲解
七.卡尔曼滤波器(UKF)开发实践七： 卡尔曼滤波器无损(UKF)进阶-实例篇 也就是本文
我介绍了上一节UKF的算法思路. 并提供了关于EKF六步公式.看公式总是枯燥的.下面，我将结合我精心设计的模拟飞机起飞和爬升的例子来完成一个完整的例子UKF开发实践.
本例基于简化UKF滤波算法(加性噪声)模型实现
模拟简单方便。 本例采用与前飞机起飞爬升的例子。 但也有一些不同(假设飞机保持固定偏航角)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 单位: 米. 这样，我们的测量值量值列向量： 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不同。卡尔曼滤波器五大公式不能直接应用；
1.因为我是使用ROS环境,所以我是在ROS下创建的一个包实现的. 只是图方便使用了ROS的CMakelist.txt,编译出可执行文件运行. 尽快我的demo程序和ros没有任何关系.;
2.因为涉及到矩阵和向量的操作.我是用的BFL库的矩阵封装API. 也是图方便系统有. 因为各个语言的矩阵API不一样.说以实现上会有不同.(#include <bfl/wrappers/matrix/matrix_wrapper.h>)
3.因为代码有注解,搭配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 count
weightVector.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‘ weights
double weight = 0.5 / (stateCount + lambda); //W_i = 1 / 2(n + λ) i = 1,2, 3, ..., n
weightVector(i) = weight;
}
#if (DEBUG)
cout << "UKF::UKF====weightVector: " << weightVector << endl;
#endif
X_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; // d
X_post(2) = 0.0; // h
X_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噪音 -> d
R_k(2, 2) = 0.0000001; // GPS噪音 -> h
R_k(3, 3) = 0.00001; // 气压计噪音噪音 -> h
R_k(4, 4) = 0.00000001; // 速度计噪音噪音 -> v
//单位矩阵，可能没用
Identity.resize(stateCount);
Identity = 0.0;
for (unsigned int i = 1; i <= stateCount; i++)
Identity(i, i) = 1;
}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噪音 -> d
R_k(2, 2) = r(2, 2); // GPS噪音 -> h
R_k(3, 3) = r(3, 3); // 气压计噪音噪音 -> h
R_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°)*Δt
F_k(2, 3) = 0.5 * deltaT; //sin(30°)*Δt
}
//动态计算B_k
void 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)^2
B_k(2) = 5 * std::pow(deltaT, 2); //0.5*sin(30°) * 20 * (Δt)^2
B_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;
}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 P
for (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.0
if( 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列
}
}
}
或
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 points
for (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_k
for (unsigned int row = 1; row <= stateCount; row++)
{
Xsig_pred(row, col) = XsigCol_2(row);
}
}
}当i=0时:
当i=0时:
void UKF::Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k)
{
//计算先验估计值： state mean
x_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点,求其方差时,使用weightforVariance4PointOne
p_k = p_k + (SymmetricMatrix)((Matrix)(weightforVariance4PointOne * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)
else
p_k = p_k + (SymmetricMatrix)((Matrix)(weightVector(col/*row*/) * x_diffMatrix) * x_diffMatrix.transpose()) + Q_k; //weightVector(i) * x_diff(nx1) * x_diff(1xn)
}
}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 readibility
double d = xsig_pred(1, col);
double h = xsig_pred(2, col);
double v = xsig_pred(3, col);
// measurement model
zsig_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 相等关系
}
}
void UKF::Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k)
{
//计算测量值均值： state mean
zz_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)
}
}
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 difference
ColumnVector 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 difference
ColumnVector 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_diff
ColumnVector Z_diff = z_k - zz_k;
//update state mean and covariance matrix
x_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 matrix
Matrix Xsig = Matrix(stateCount, sigmaPointsCount);
//Step 1
Step1_GenerateSigmaPoints(X_post, P_post, Xsig);
//create predict sigma point matrix
Matrix Xsig_pred = Matrix(stateCount, sigmaPointsCount);
//Step 2
Step2_PredictSigmaPointsByStateFunction(Xsig, Xsig_pred, deltaT);
//Step 3
Step3_CalculateMeanAndCovarianceByWeight(Xsig_pred, X_k, P_k);
//Step 4
Matrix Zsig_pred = Matrix(measurementCount, sigmaPointsCount);
Step4_PredictSigmaPointsByMeasurementFunction(Xsig_pred, Zsig_pred);
//Step 5
ColumnVector 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 6
Step6_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;//s
UKF 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噪音方差(协方差) -> d
R(2, 2) = 0.0000001; // GPS噪音方差(协方差) -> h
R(3, 3) = 0.00001; // 气压计噪音噪音方差(协方差) -> h
R(4, 4) = 0.00000001; // 速度计噪音噪音方差(协方差) -> v
ukf.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;
}1.速度计测量计和最优估计速度值曲线(因为设置速度计测量噪音很小,所有两者几乎重合)
2.GPS测量高度值, 气压计测量高度值,和最优估计高度值曲线图.
可见,由于气压计测量的噪音误差较大, 三者之间的曲线由没有交错现象
3.GPS测量前进位置和最优估计位移曲线
因为GPS测测量噪音很小, 所有两者也是几乎重合
有不足之处欢迎留言指正,共同进步,谢谢!
另外别忘了点个赞哦.