clear clc close all load('data1.mat'); nn = size(imuPosX,1); %% lightHouse坐标系转换 x = lightHousePosX * 100; y = -lightHousePosZ * 100; % 定义误差函数，即均方根误差 errorFunction = @(params) sqrt(mean((x(1:500) * cos(params(1)) - y(1:500) * sin(params(1)) + params(2) - imuPosX(1:500)).^2 + (x(1:500) * sin(params(1)) + y(1:500) * cos(params(1)) + params(3) - imuPosY(1:500)).^2)); % 使用 fminsearch 优化误差函数，找到使误差最小的旋转角和位移 initialGuess = [0, 10, 10]; % 初始猜测值，[旋转角, 位移] optimizedParams = fminsearch(errorFunction, initialGuess); % 输出优化后的旋转角和位移 rotationAngle = optimizedParams(1); xOffset = optimizedParams(2); yOffset = optimizedParams(3); % 旋转坐标 xt = x * cos(rotationAngle) - y * sin(rotationAngle) + xOffset; yt = x * sin(rotationAngle) + y * cos(rotationAngle) + yOffset; %% tagN = 4; %标签坐标 XN(:,1)=[-300;-300]; XN(:,2)=[-300;300]; XN(:,3)=[300;300]; XN(:,4)=[300;-300]; sim2=6; Q=diag(repmat(sim2,1,2*tagN));%协方差矩阵 measure_AOA = zeros(4,nn); measure_d = zeros(4,nn); measure_AOA(1,:) = aoa1'; measure_AOA(2,:) = aoa2'; measure_AOA(3,:) = aoa3'; measure_AOA(4,:) = aoa4'; measure_d(1,:) = d1'; measure_d(2,:) = d2'; measure_d(3,:) = d3'; measure_d(4,:) = d4'; %% uwb解算 x_uwb(1) = 0; y_uwb(1) = 0; theta_uwb=zeros(nn,1); for i=2:nn if measure_d(1,i) == 0 x_uwb(i) = x_uwb(i-1); y_uwb(i) = y_uwb(i-1); continue; end [t1,theta] = WLS(XN,measure_AOA(:,i),measure_d(:,i),Q); x_uwb(i) = t1(1); y_uwb(i) = t1(2); theta_uwb(i) = 90-theta; theta_uwb(i) = mod(theta_uwb(i)+180,360)-180; derr_WLS(i)=norm(t1-[xt(i);yt(i)]); end thetat=zeros(nn,1); for i=2:nn detx=xt(i)-xt(i-1); dety=yt(i)-yt(i-1); thetat(i)=atan2d(detx,dety); end % Uwb.x=x_uwb; % Uwb.y=y_uwb; % Uwb.alpha=theta_uwb; % 角速度校准 detW = mean(wZ(1:200)); wZ = wZ - detW; x_imu(1) = 0; y_imu(1) = 0; Z=zeros(3,1); WW = 0.05; theta_imu(1) = 0; % % Imu.x=x_imu; % Imu.y=y_imu; % Imu.v=v_imu; % Imu.alpha=alpha_imu; % Imu.omega=omega_imu; %% KF R = diag([1 1 1]); % qq = 1; qq = 0.00001; Q1 = diag([qq qq qq qq qq]); P0 = diag([0 0 0 0 0]); H = [1 0 0 0 0; 0 1 0 0 0; 0 0 0 1 0]; I = eye(5); JF = zeros(5,5); X_pre = zeros(5,nn); X_kf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)]; for i=2:nn % 计算IMU和里程计的时间差值 detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1); detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1); % 获得此时Z轴角速度 w = wZ(i); % 获得小车的水平速度 v = vXY(i)*100; % 计算速度增量 detV=(vXY(i)-vXY(i-1))*100; % 计算位置增量 detD = v*detImuTime; % 计算角度增量 detTheta = w*180/pi*detImuTime; % 计算角速度的变化量 detW = w-wZ(i-1); % 积分计算IMU的航位角 theta_imu(i) = theta_imu(i-1)+detTheta; % 航向约束 theta_imu(i) = mod(theta_imu(i)+180,360)-180; % 状态更新方程 F = [1 0 detImuTime*cosd(X_kf(4,i-1)) 0 0; 0 1 detImuTime*sind(X_kf(4,i-1)) 0 0; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0;]; % 获得水平和垂直方向的位置增量 vtx = detD*cosd(theta_imu(i)); vty = detD*sind(theta_imu(i)); x_imu(i) = x_imu(i-1)+vtx; y_imu(i) = y_imu(i-1)+vty; X_next = [vtx;vty;detV;detTheta;detW]; if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1)) X_kf(:,i) = X_kf(:,i-1)+X_next; errKf(i) = norm(X_kf(1:2,i)-[xt(i);yt(i)]); theta_kf(i) = X_kf(4,i); continue end X_pre(:,i) = X_kf(:,i-1)+X_next; Z = [x_uwb(i);y_uwb(i);theta_uwb(i)]; P = F*P0*F'+Q1; Kg_kf = P*H'*inv(H*P*H'+R); X_kf(:,i) = X_pre(:,i)+Kg_kf*(Z-H*X_pre(:,i)); P0 = (I-Kg_kf*H)*P; errKf(i) = norm(X_kf(1:2,i)-[xt(i);yt(i)]); theta_kf(i) = X_kf(4,i); end %% EKF R = diag([1 1 1]); % qq = 1; qq = 0.00001; Q1 = diag([qq qq qq qq qq]); P0 = diag([0 0 0 0 0]); H = [1 0 0 0 0; 0 1 0 0 0; 0 0 0 1 0]; KK = zeros(5,3); X_ekf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)]; I = eye(5); JF = zeros(5,5); X_pre = zeros(5,nn); for i=2:nn detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1); detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1); w = wZ(i); v = vXY(i)*100; detV=(vXY(i)-vXY(i-1))*100; detD = v*detImuTime; detTheta = w*180/pi*detImuTime; detW = w-wZ(i-1); theta_imu(i) = theta_imu(i-1)+detTheta; theta_imu(i) = mod(theta_imu(i)+180,360)-180; F = [1 0 detImuTime*cosd(X_ekf(4,i-1)) 0 0; 0 1 detImuTime*sind(X_ekf(4,i-1)) 0 0; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0;]; if w15 % xxx = 1; % end end errAEKf_sw=zeros(1,nn); weightedvector=[0.3;0.25;0.2;0.15;0.1]; for i=1:nn if i>windowlength-1 x_win=X_aekf(1,i-windowlength+1:i); y_win=X_aekf(2,i-windowlength+1:i); x_aekf_sw(i)=x_win*weightedvector; y_aekf_sw(i)=y_win*weightedvector; else x_aekf_sw(i)=X_aekf(1,i); y_aekf_sw(i)=X_aekf(2,i); end errAEKf_sw(i) = norm([x_aekf_sw(i);y_aekf_sw(i)]-[xt(i);yt(i)]); end figure; plot(errAEKf_sw); %% for i=2:length(theta_uwb)-1 if theta_uwb(i) == 0 theta_uwb(i) = theta_uwb(i-1); end end figure(1) clf(1) plot(x_uwb,y_uwb,'.','color',[205/255, 133/255, 63/255],'LineWidth',0.2);hold on plot(x_imu, y_imu, '-','color',[0, 128/255, 0],'LineWidth',0.5); plot(xt, yt, '-black','LineWidth',0.5); legend('AUAM','IMU','True value'); xlabel('X(cm)'); ylabel('Y(cm)'); grid on % figure(1) % clf(1) % hold on % plot((1:nn)/100,thetat,'-black','LineWidth',1); % plot((1:nn)/100,theta_uwb,'-','color','#CD853F','LineWidth',1); % plot((1:nn)/100,theta_imu, '-','color','#008000','LineWidth',1); % legend('LightHouse','UWB', 'INS'); % xlabel('time(s)'); % ylabel('\alpha(°)'); % grid on % figure(2) % clf(2) % hold on % plot(theta_imu,'.'); % plot(theta_uwb,'.'); % legend('imu','uwb'); % xlabel('运行时间（10ms）'); % ylabel('偏航角（°）'); % title('角速度校准后imu和uwb测量偏航角'); % grid on % % figure(3) % clf(3) % hold on % plot(xt, yt, '.-black','LineWidth',1); % plot(X_ekf(1,:), X_ekf(2,:), '.-','color','#7E2F8E','LineWidth',1); % plot(X_aekf(1,:), X_aekf(2,:), '.-','color','#D95319','LineWidth',0.5) % plot(x_aekf_sw, y_aekf_sw, '.-','color','#2E8B57','LineWidth',0.5) % legend('True','EKF', 'AEKF', 'AEKF-SWF'); % xlabel('x(cm)'); % ylabel('y(cm)'); % grid on % % figure(4) % clf(4) % hold on % plot(errKf,'.','color','#0072BD'); % plot(errEKf,'.','color','#EDB120'); % plot(errUKf,'.','color','#D95319'); % plot(errAEKf,'.','color','#7E2F8E'); % plot(errAEKf_sw,'.','color','#77AC30'); % legend('KF','EKF','UKF','AEKF','AEKF-SWF'); % xlabel('运行时间（10ms）'); % ylabel('误差（cm）'); % title('定位误差随时间变化图'); % slt = 1; % uwbcount = 9; % imucount = 15; % ekfcount = 12; % aekfcount = 5; % figure(5) % clf(5) % hold on % [fUWB,xUWB] = ksdensity(errUwb(slt:end)); % [fIMU,xIMU] = ksdensity(errImu(slt:end)); % [fAEKF,xAEKF] = ksdensity(errAEKf(slt:end)); % [fUKF,xUKF] = ksdensity(errUKf(slt:end)); % [fAEKF_sw,xAEKF_sw] = ksdensity(errAEKf_sw(slt:end)); % [fEKF,xEKF] = ksdensity(errEKf(slt:end)); % [fKF,xKF] = ksdensity(errKf(slt:end)); % plot(xKF(ekfcount:end),fKF(ekfcount:end), 'color','#0072BD','LineWidth',0.5); % plot(xEKF(ekfcount:end),fEKF(ekfcount:end),'color','#EDB120','LineWidth',0.5); % plot(xUKF(ekfcount:end), fUKF(ekfcount:end),'color','#D95319','LineWidth',0.5); % plot(xAEKF(aekfcount:end),fAEKF(aekfcount:end),'color','#7E2F8E','LineWidth',0.5); % plot(xAEKF_sw(aekfcount:end),fAEKF_sw(aekfcount:end), 'color','#77AC30','LineWidth',0.5) % legend('KF','EKF','UKF','AEKF','AEKF-SWF');%'UWB','INS', % xlabel('Error(cm)'); % ylabel('Probability(%)'); % grid on % % % % slt = 3000; % uwbcount = 9; % imucount = 9; % ekfcount = 13; % aekfcount = 7; % figure(6) % clf(6) % hold on % [fAEKF,xAEKF] = ksdensity(errAEKf(slt:end)); % [fUKF,xUKF] = ksdensity(errUKf(slt:end)); % [fAEKF_sw,xAEKF_sw] = ksdensity(errAEKf_sw(slt:end)); % [fEKF,xEKF] = ksdensity(errEKf(slt:end)); % [fKF,xKF] = ksdensity(errKf(slt:end)); % plot(xKF(ekfcount:end),fKF(ekfcount:end), 'color','#0072BD','LineWidth',0.5); % plot(xEKF(ekfcount:end),fEKF(ekfcount:end),'color','#EDB120','LineWidth',0.5); % plot(xUKF(ekfcount:end), fUKF(ekfcount:end),'color','#D95319','LineWidth',0.5); % plot(xAEKF(aekfcount:end),fAEKF(aekfcount:end),'color','#7E2F8E','LineWidth',0.5); % plot(xAEKF_sw(aekfcount:end),fAEKF_sw(aekfcount:end), 'color','#77AC30','LineWidth',0.5) % legend('KF','EKF','UKF','AEKF','AEKF-SWF');%'UWB','INS', % xlabel('Error(cm)'); % ylabel('PDF'); % grid on % % figure(7) % clf(7) % hold on % h1 = cdfplot(errKf(slt:end)); % h2 = cdfplot(errEKf(slt:end)); % h3 = cdfplot(errUKf(slt:end)); % h4 = cdfplot(errAEKf(slt:end)); % h5 = cdfplot(errAEKf_sw(slt:end)); % set(h1,'color','#0072BD','LineWidth',0.5); % set(h2,'color','#EDB120','LineWidth',0.5); % set(h3,'color','#D95319','LineWidth',0.5); % set(h4,'color','#7E2F8E','LineWidth',0.5); % set(h5,'color','#77AC30','LineWidth',0.5); % legend('KF','EKF','UKF','AEKF','AEKF-SWF'); % xlabel('Error(cm)'); % ylabel('CDF'); % grid on