Table of contents
1. Preview of algorithm operation renderings
2.Algorithm running software version
4. Overview of algorithm theory
5. Algorithm complete program engineering
1. Preview of algorithm operation renderings
2.Algorithm running software version
matlab2022a
3. Some core programs
..........................................................................
kkk = 0;
for EbN0 = EbN0_sub
kkk
kkk = kkk + 1;
for jj1 = 1:Tag_Num
jj1
rng(jj1);
for jj = 1:num_bits
%TAG to BS1
delay_1 = round(time_bs_tag(1,jj1)/ts);
xx1 = zeros(1,delay_1);
%传播时延
delay_1_1(jj,:) = [xx1 sig(1:end-length(xx1))];
%UWB
h_4 = uwb_channel(dist_bs_tag(1,jj1));
%信号经过信道
conv_data1 = conv(delay_1_1(jj,:),h_4);
UWB_chan1(jj,:) = conv_data1(1:length(sig));
%TAG to BS2
delay_2 = round(time_bs_tag(2,jj1)/ts);
xx2 = zeros(1,delay_2);
%传播时延
delay_2_1(jj,:) = [xx2 sig(1:end-length(xx2))];
h_2 = uwb_channel(dist_bs_tag(2,jj1));
conv_data2 = conv(delay_2_1(jj,:),h_2);
UWB_chan2(jj,:) = conv_data2(1:length(sig));
%TAG to BS3
delay_3 = round(time_bs_tag(3,jj1)/ts);
xx3 = zeros(1,delay_3);
%传播时延
delay_3_1(jj,:) = [xx3 sig(1:end-length(xx3))];
h_3 = uwb_channel(dist_bs_tag(3,jj1));
conv_data3 = conv(delay_3_1(jj,:),h_3);
UWB_chan3(jj,:) = conv_data3(1:length(sig));
%TAG to BS4
delay_4 = round(time_bs_tag(4,jj1)/ts);
xx4 = zeros(1,delay_4);
%传播时延
delay_4_1(jj,:) = [xx4 sig(1:end-length(xx4))];
h_4 = uwb_channel(dist_bs_tag(4,jj1));
conv_data4 = conv(delay_4_1(jj,:), h_4);
UWB_chan4(jj,:) = conv_data4(1:length(sig));
end
for jj = 1:num_bits
UWB_chan1n(jj,:) = awgn(UWB_chan1(jj,:)/max(UWB_chan1(jj,:)),EbN0,'measured');
UWB_chan2n(jj,:) = awgn(UWB_chan2(jj,:)/max(UWB_chan2(jj,:)),EbN0,'measured');
UWB_chan3n(jj,:) = awgn(UWB_chan3(jj,:)/max(UWB_chan3(jj,:)),EbN0,'measured');
UWB_chan4n(jj,:) = awgn(UWB_chan4(jj,:)/max(UWB_chan4(jj,:)),EbN0,'measured');
end
%自适应前沿检测
%自适应前沿检测
..........................................................
end
end
P_est0 = [x_est0',y_est0',z_est0'];
P_est1 = [x_est1',y_est1',z_est1'];
figure;
plot(toa_error0,'-r>',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.9,0.9,0.0]);
hold on
title('估计误差')
axis([0,Tag_Num,0,2]);
ylabel('cm');
figure
axis([0 10 0 10 0 10]);
for i=1:BS_Num
plot3(BS_pos(i,1),BS_pos(i,2),BS_pos(i,3),'ko','MarkerFace','y','MarkerSize',8);
hold on
end
hold on
for i=1:Tag_Num
plot3(Tag(i,1),Tag(i,2),Tag(i,3),'k^','MarkerFace','b','MarkerSize',6);
hold on
plot3(x_est1(i),y_est1(i),z_est1(i),'ks','MarkerFace','r','MarkerSize',6);
hold on
end
grid on
xlabel('cm');
ylabel('cm');
zlabel('cm');
save R.mat toa_error1
36_003m4. Overview of algorithm theory
The three-dimensional spatial positioning algorithm based on the fusion of UWB and IMU is a positioning method that combines the respective advantages of wireless pulse wave (UWB) and inertial measurement unit (IMU). UWB calculates distance by measuring the transmission time of the signal. It has the advantages of high accuracy and strong anti-interference ability, but it is susceptible to multipath effects and environmental noise. The IMU calculates attitude and position information by measuring acceleration and angular velocity, which is real-time and dynamic, but is limited by acceleration measurement errors and drift.
By fusing these two technologies, their advantages can be fully utilized to improve positioning accuracy and stability. Specifically, UWB can provide high-precision distance information for calculating the target's position and attitude, while IMU can provide real-time acceleration and angular velocity information to correct UWB measurement errors and drift, while improving the system's response speed and Robustness.
The following introduces a three-dimensional space positioning algorithm based on the fusion of UWB and IMU. Its principle and mathematical formula are as follows:
- UWB positioning
UWB adopts the positioning method of dual base stations. Assuming that the position coordinates of the two base stations are (x1, y1, z1) and (x2, y2, z2), and the position coordinates of the target are (x, y, z), you can The following formula calculates the distance difference between the target and the two base stations:
Δd = (x2-x1)² + (y2-y1)² + (z2-z1)² - (x-x1)² - (y-y1)² - (z-z1)²
Among them, (x, y, z) are the position coordinates of the target, (x1, y1, z1) and (x2, y2, z2) are the position coordinates of the two base stations respectively. Based on the distance difference and the coordinates of the two base stations, two equations can be listed and solved to obtain the target's position coordinates (x, y, z).
IMU assist
The IMU can provide real-time acceleration and angular velocity information to correct UWB measurement errors and drift. Specifically, the IMU can provide an acceleration sensor and a gyroscope to measure acceleration and angular velocity information respectively. By integrating and smoothing this information, the attitude and position information of the target can be obtained.
During the fusion process, the acceleration and angular velocity information of the IMU can be used as UWB auxiliary data to correct the UWB measurement results. Specifically, the acceleration information of the IMU can be used to calculate the speed and acceleration of the target to correct the UWB distance measurement results, and the angular velocity information of the IMU can be used to correct the UWB angle measurement results. This results in higher accuracy and robustness of the system.
Fusion algorithm
The three-dimensional spatial positioning algorithm based on the fusion of UWB and IMU mainly includes two stages: data acquisition stage and data fusion stage. In the data collection stage, the target's position, velocity, acceleration, angular velocity and other information are collected through UWB and IMU; in the data fusion stage, the collected data are fused to obtain the target's final position, velocity, acceleration, angular velocity and other information.
The solution process can be optimized using least squares method, Kalman filter and other methods as needed. For example, the Kalman filter algorithm can be used to fuse UWB and IMU data to obtain more accurate target position, speed, acceleration, angular velocity and other information. The specific implementation process is as follows:
(1) Initialize the state matrix and control matrix;
(2) Collect data through UWB and IMU;
(3) Use the collected data to calculate the state matrix and control matrix;
(4) Calculate the state matrix and control matrix according to the Kalman filter formula Iterative calculation;
(5) Calculate the final position, speed, acceleration, angular velocity and other information of the target based on the iterative results.
Algorithm advantages
The three-dimensional space positioning algorithm based on the fusion of UWB and IMU has the following advantages:
(1) High accuracy: Through the fusion of UWB and IMU, the impact of environmental noise on positioning accuracy can be reduced and the robustness of the algorithm can be improved;
(2) Real-time performance Strong: The acceleration and angular velocity information of the IMU can provide real-time attitude and position information, correct the UWB distance measurement results, and shorten the response time of the system; (3) High reliability: through data fusion technology to process multi-sensor data, it
can Reduce the impact of a single sensor failure on system performance;
(4) Strong scalability: This algorithm can be applied to a variety of scenarios, such as robot positioning, driverless driving, etc.
5. Algorithm complete program engineering
OOOOO
OOO
O