UWB-IMU、UWB定位对比研究（Matlab代码实现）

程序猿鑫

于 2024-10-11 11:42:00 发布

阅读量642

点赞数 13

文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/ke_yan_she/article/details/142849934

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、详细文档

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

IMU的误差模型采用一阶马尔科夫噪声模型，将加速度计和陀螺仪噪声建立为高斯白噪声和Guass-Markov噪声。

结论：UWB-IMU组合定位导航效果，比之单一的导航，效果很明显，尤其是当UWB布局上无法解决垂直空间分辨率低时，融合算法效果明显。目前使用实际数据，效果亦能控制在0.5m以内，定位精度还可以提升，已证明该滤波方法可靠有效。

📚2 运行结果

部分代码：

%%航迹发生器
atti = zeros(3,1); %滚转、俯仰、偏航（单位：度）
atti_rate = zeros(3,1); %滚转角速率、俯仰角速率、偏航角速率（单位：度/秒）
veloB = zeros(3,1); %飞机运动速度--X右翼、Y机头、Z天向（单位：米/秒）
acceB = zeros(3,1); %飞机运动加速度--X右翼、Y机头、Z天向（单位：米/秒/秒）
posi = zeros(3,1); %航迹发生器初始位置经度、纬度、高度（单位：度、度、米）
posi = [26;70;-19];

atti(1,1) = 0; %
atti(2,1) = 0; %
atti(3,1) = 90; %初始航向角（单位：度）

%%IMU输出
Wibb = zeros(3,1); %机体系陀螺仪输出（单位：度/秒）
Fb = zeros(3,1); %机体系加速度计输出（单位：米/秒/秒）
Gyro_fix = zeros(3,1); %机体系陀螺仪固定误差输出（单位：弧度/秒）
Acc_fix = zeros(3,1); %机体系加速度计固定误差输出（单位：米/秒/秒）
Gyro_b = zeros(3,1); %陀螺随机常数（弧度/秒）
Gyro_r = zeros(3,1); %陀螺一阶马尔可夫过程（弧度/秒）
Gyro_wg = zeros(3,1); %陀螺白噪声（弧度/秒）
Acc_r = zeros(3,1); %加速度一阶马尔可夫过程（米/秒）

%%UWB仿真输出
posiG = zeros(3,1); %UWB输出的飞行器位置测距值

%%捷联惯导仿真
attiN = zeros(3,1); %飞行器初始姿态
veloN = zeros(3,1); %飞行器初始速度（相对于导航系）
posiN = zeros(3,1); %飞行器初始位置
WnbbA_old = zeros(3,1); %角速度积分输出（单位：弧度）

posiN = posi;
attiN = atti;

%%KALMAN滤波输出
T_D = 1; %离散周期
T_M = 0; %滤波量测产生时间（秒）
Xc = zeros(18,1); %综合模型状态量
PK = zeros(18,18); %协方差阵
Xerr = zeros(1,18); %状态估计量的误差
kflag = 0; %GPS信号有效标志位（1-有效）

Acc_modi = zeros(3,1); %加速度计误差修正值
Gyro_modi = zeros(3,1); %陀螺仪误差修正值

%%初始对准
kc = 0;
tmp_Fb = zeros(3,1);
tmp_Wibb = zeros(3,1);
t_alig = 0;

old_veloB = veloB;
old_atti = atti;

deg_rad = pi/180;

TraceData = [];
IMUData =[];