diff --git a/Code/Matlab/AOA-IMU/RUN_AEKF.m b/Code/Matlab/AOA-IMU/RUN_AEKF.m
new file mode 100644
index 0000000..a64e542
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/RUN_AEKF.m
@@ -0,0 +1,580 @@
+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 w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    x_imu(i) = x_imu(i-1)+vtx; 
+    y_imu(i) = y_imu(i-1)+vty;
+    errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
+    errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
+    X_next = [vtx;vty;detV;detTheta;detW];
+    if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
+        X_ekf(:,i) = X_ekf(:,i-1)+X_next;
+        errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
+        theta_ekf(i) = X_ekf(4,i);
+        continue
+    end    
+    if w<WW
+        X_pre(:,i) = X_ekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = F*P0*F'+Q1;
+        Kg = P*H'*inv(H*P*H'+R);
+        X_ekf(:,i) = X_pre(:,i)+Kg*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    else
+        JF = [1 0 1/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) v/w*(cosd(theta_imu(i))-cosd(theta_imu(i-1))) detD/w*cosd(theta_imu(i))-v/(w^2)*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+             0 1 1/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1))) v/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) detD/w*sind(theta_imu(i))-v/(w^2)*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
+             0 0 1 0 0;
+             0 0 0 1 detImuTime;
+             0 0 0 0 1];
+
+        X_pre(:,i) = X_ekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = JF*P0*JF'+Q1;
+        Kg = P*H'*inv(H*P*H'+R);
+        X_ekf(:,i) = X_pre(:,i)+Kg*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    end
+    errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
+    theta_ekf(i) = X_ekf(4,i);
+end
+%%  UKF
+% UKF settings
+ukf_L = 5; %numer of states
+ukf_m = 3; %numer of measurements
+ukf_kappa = 3 - ukf_L;
+ukf_alpha = 0.9;
+ukf_beta = 2;
+
+ukf_lambda = ukf_alpha^2*(ukf_L + ukf_kappa) - ukf_L;
+ukf_gamma = sqrt(ukf_L + ukf_lambda);
+ukf_W0_c = ukf_lambda / (ukf_L + ukf_lambda) + (1 - ukf_alpha^2 + ukf_beta);
+ukf_W0_m = ukf_lambda / (ukf_L + ukf_lambda);
+ukf_Wi_m = 1 / (2*(ukf_L + ukf_lambda));
+ukf_Wi_c = ukf_Wi_m;
+
+q=0.01;    %std of process 
+r=5;    %std of measurement
+p=2;
+Qu=q*eye(ukf_L); % std matrix of process
+Ru=r*eye(ukf_m);        % std of measurement 
+Pu=p*eye(ukf_L);
+H = [1 0 0 0 0;
+     0 1 0 0 0;
+     0 0 0 1 0];
+X_ukf(:,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 w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    x_imu(i) = x_imu(i-1)+vtx; 
+    y_imu(i) = y_imu(i-1)+vty;
+    errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
+    errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
+    X_next = [vtx;vty;detV;detTheta;detW];
+    if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
+        X_ukf(:,i) = X_ukf(:,i-1)+X_next;
+        errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
+        theta_ukf(i) = X_ukf(4,i);
+        continue
+    end    
+
+    xestimate = X_ukf(:,i-1);
+    Xx = repmat(xestimate, 1, length(xestimate));
+    Xsigma = [xestimate, ( Xx + ukf_gamma * Pu ), ( Xx - ukf_gamma * Pu )];
+    
+    %第二步：对Sigma点集进行一步预测
+    [Xsigmapre]=fun1(Xsigma,detImuTime);
+    %第三步：估计预测状态
+    Xpred = ukf_W0_m * Xsigmapre(:,1) + ukf_Wi_m * sum(Xsigmapre(:,2:end), 2);
+    %第四步：均值和方差
+      Xx = repmat(Xpred, 1, length(xestimate)*2);
+     [~, R] = qr([sqrt(ukf_Wi_c) * ( Xsigmapre(:,2:end) - Xx ), Qu]', 0);
+     Ppred = cholupdate(R, sqrt(ukf_W0_c) * (Xsigmapre(:,1) - Xpred), '-')';
+
+    %第5步：根据预测值，再一次使用UT变换，得到新的sigma点集
+    Xx = repmat(Xpred, 1, length(xestimate));
+    Xsigmapre = [Xpred, ( Xx + ukf_gamma * Ppred ), ( Xx - ukf_gamma * Ppred )];
+
+    %第6步：观测预测
+    Zsigmapre=H*Xsigmapre;
+
+    %第7步：计算观测预测均值和协方差
+    Zpred = ukf_W0_m * Zsigmapre(:,1) + ukf_Wi_m * sum(Zsigmapre(:,2:end), 2);
+
+    Yy = repmat(Zpred, 1, length(xestimate)*2);
+    [~, Rz] = qr([sqrt(ukf_Wi_c) * (Zsigmapre(:,2:end) - Yy), Ru]', 0);
+    Pzz = cholupdate(Rz, sqrt(ukf_W0_c) * (Zsigmapre(:,1) - Zpred), '-')';
+
+    Xd = Xsigmapre - repmat(Xpred, 1, length(xestimate)*2 + 1);
+    Zd = Zsigmapre - repmat(Zpred, 1, length(xestimate)*2 + 1);
+  
+    Pxz = ( ukf_W0_c*  Xd(:,1) * Zd(:,1)' ) + ( ukf_Wi_c * Xd(:,2:end) * Zd(:,2:end)' );
+
+    %第七步：计算kalman增益
+    Kukf= (Pxz / Pzz') / Pzz;
+    %第八步：状态和方差更新
+    Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+    Xpred=Xpred+Kukf*(Z-Zpred);
+    U = Kukf * Pzz;
+    Rp = Ppred';
+    for ii=1:size(U, 2)
+        Rp = cholupdate(Rp, U(:,ii), '-');
+    end
+    Ppred = Rp';
+    X_ukf(:,i)=Xpred;
+
+    errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
+    theta_ukf(i) = X_ukf(4,i);
+end
+
+%%
+%%AEKF
+R = diag([1 1 1]);
+% qq = 50;
+% qq = 10;
+qq = 0.01;
+Q = 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];
+X_aekf = zeros(5,nn);
+X_aekf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
+I = eye(5);
+JF = zeros(5,5);
+X_pre = zeros(5,nn);
+alfa = 0.97;
+
+windowlength=5;
+x_aekf_sw=zeros(1,nn);
+y_aekf_sw=zeros(1,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_aekf(4,i-1)) 0 0;
+         0 1 detImuTime*sind(X_aekf(4,i-1)) 0 0;
+         0 0 0 0 0;
+         0 0 0 0 0;
+         0 0 0 0 0;];
+    if w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    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_aekf(:,i) = X_aekf(:,i-1)+X_next;
+        errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
+        theta_aekf(i) = X_aekf(4,i);
+        continue
+    end    
+    if w<WW
+        X_pre(:,i) = X_aekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = F*P0*F'+Q;
+        Kg = P*H'*inv(H*P*H'+R);
+        KK(1,1) = Kg(1,1);
+        KK(2,2) = Kg(2,2);
+        KK(4,3) = Kg(4,3);
+        X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    else
+        JF = [1 0 1/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) v/w*(cosd(theta_imu(i))-cosd(theta_imu(i-1))) detD/w*cosd(theta_imu(i))-v/(w^2)*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+             0 1 1/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1))) v/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) detD/w*sind(theta_imu(i))-v/(w^2)*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
+             0 0 1 0 0;
+             0 0 0 1 detImuTime;
+             0 0 0 0 1];
+        X_pre(:,i) = X_aekf(:,i-1)+X_next;
+        dk = Z - H*X_pre(:,i);
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = JF*P0*JF'+Q;
+        Kg = P*H'*inv(H*P*H'+R);
+        KK(1,1) = Kg(1,1);
+        KK(2,2) = Kg(2,2);
+        KK(4,3) = Kg(4,3);
+        X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
+        epz = Z-H*X_aekf(:,i);
+        R=alfa*R+(1-alfa)*(epz*epz'+H*P*H');
+        Q=alfa*Q+(1-alfa)*Kg*dk*dk'*Kg';
+        P0 = (I-Kg*H)*P;
+    end
+    errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
+    theta_aekf(i) = X_aekf(4,i);
+    % if errAEKf(i)>15
+    %     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
\ No newline at end of file
diff --git a/Code/Matlab/AOA-IMU/WLS.m b/Code/Matlab/AOA-IMU/WLS.m
new file mode 100644
index 0000000..f744c9a
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/WLS.m
@@ -0,0 +1,33 @@
+function [x,theta] = WLS(XN,mean_aoa,mean_d,Q)
+%PLE 此处显示有关此函数的摘要
+%   此处显示详细说明
+nn = size(XN,2);
+A = zeros(2*nn, 6);
+b = zeros(2*nn, 1);
+for j=1:nn
+    XN_Tag(1,j)=mean_d(j)*sind(mean_aoa(j));
+    XN_Tag(2,j)=mean_d(j)*cosd(mean_aoa(j));
+    A(2*j-1,:)  = [XN_Tag(1,j) 0 XN_Tag(2,j) 0 1 0];
+    A(2*j,:)    = [0 XN_Tag(1,j) 0 XN_Tag(2,j) 0 1];
+    b(2*j-1:2*j)=[XN(1,j) XN(2,j)];     
+end 
+f=(A'*A)^(-1)*A'*b;
+for i=1:2
+    for j=1:nn
+        B(2*j-1,2*j-1) = XN(2,j)-f(6);
+        B(2*j,2*j) = XN(1,j)-f(5);
+    end
+  W = inv(B*Q*B');
+  f=(A'*W*A)\A'*W*b;
+end
+x=(f(5:6));
+theta1(1)=atan2d(-f(2),f(1));
+theta1(2)=atan2d(f(3),f(4));
+% theta1(1)=acosd(-f(1));
+% theta1(2)=asind(-f(2));
+% theta1(3)=asind(f(3));
+% theta1(4)=acosd(-f(4));
+
+theta = mean(theta1);
+end
+
diff --git a/Code/Matlab/AOA-IMU/data1.mat b/Code/Matlab/AOA-IMU/data1.mat
new file mode 100644
index 0000000..fe1e94a
Binary files /dev/null and b/Code/Matlab/AOA-IMU/data1.mat differ
diff --git a/Code/Matlab/AOA-IMU/fun1.m b/Code/Matlab/AOA-IMU/fun1.m
new file mode 100644
index 0000000..90f93a7
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/fun1.m
@@ -0,0 +1,28 @@
+function [Y] = fun1(X,dt)
+%FUN 此处显示有关此函数的摘要
+%   此处显示详细说明
+WW = 0.05;
+nn = size(X,2);
+Y = zeros(5,nn);
+
+for i=1:nn
+    v = X(3,i);
+    alfa = X(4,i);
+    w = X(5,i)*180/pi;
+    if w<WW
+        alfa = alfa+w*dt;
+        Y(:,i) = [X(1,i) + v*dt*cosd(alfa);
+                  X(2,i) - v*dt*sind(alfa);
+                  v;
+                  alfa;
+                  w*pi/180];        
+    else
+        Y(:,i) = [X(1,i) + v/w*(sind(alfa+w*dt)-sind(alfa));
+                  X(2,i) - v/w*(cosd(alfa+w*dt)-cosd(alfa));
+                  v;
+                  alfa+w*dt;
+                  w*pi/180];
+    end
+end
+end
+
diff --git a/Code/Matlab/AOA-IMU/script.m b/Code/Matlab/AOA-IMU/script.m
new file mode 100644
index 0000000..d7b945d
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/script.m
@@ -0,0 +1,36 @@
+
+
+clear
+clc
+close all
+load('data1.mat');
+nn = size(imuPosX,1);
+%% lightHouse坐标系转换
+x =  lightHousePosX * 100;
+y = -lightHousePosZ * 100;
+
+plot(x,y,'r',imuPosX,imuPosY);
+
+% 定义误差函数，即均方根误差
+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;
+
+plot(xt,yt,'r',imuPosX,imuPosY);
+
+ds = zeros(1,length(imuDataRxTime));
+for i = 1:length(imuDataRxTime)-1
+    ds(i) = 1 / (imuDataRxTime(i+1)-imuDataRxTime(i));
+end
+plot(ds)
+
diff --git a/Code/Matlab/AOA-IMU/script_aekf.m b/Code/Matlab/AOA-IMU/script_aekf.m
new file mode 100644
index 0000000..b2a0554
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/script_aekf.m
@@ -0,0 +1,199 @@
+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;
+
+
+%%  EKF
+KK = zeros(5,3);
+
+%%
+%%AEKF
+R = diag([1 1 1]);
+% qq = 50;
+% qq = 10;
+qq = 0.01;
+Q = 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];
+X_aekf = zeros(5,nn);
+X_aekf(:,1) = [imuPosX(1);imuPosY(1);vXY(1)*100;0;wZ(1)];
+I = eye(5);
+JF = zeros(5,5);
+X_pre = zeros(5,nn);
+alfa = 0.97;
+
+windowlength=5;
+x_aekf_sw=zeros(1,nn);
+y_aekf_sw=zeros(1,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_aekf(4,i-1)) 0 0;
+         0 1 detImuTime*sind(X_aekf(4,i-1)) 0 0;
+         0 0 0 0 0;
+         0 0 0 0 0;
+         0 0 0 0 0;];
+     
+    if w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    
+    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_aekf(:,i) = X_aekf(:,i-1)+X_next;
+        errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
+        theta_aekf(i) = X_aekf(4,i);
+        continue
+    end    
+    if w<WW
+        X_pre(:,i) = X_aekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = F*P0*F'+Q;
+        Kg = P*H'*inv(H*P*H'+R);
+        KK(1,1) = Kg(1,1);
+        KK(2,2) = Kg(2,2);
+        KK(4,3) = Kg(4,3);
+        X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    else
+        JF = [1 0 1/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) v/w*(cosd(theta_imu(i))-cosd(theta_imu(i-1))) detD/w*cosd(theta_imu(i))-v/(w^2)*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+             0 1 1/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1))) v/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) detD/w*sind(theta_imu(i))-v/(w^2)*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
+             0 0 1 0 0;
+             0 0 0 1 detImuTime;
+             0 0 0 0 1];
+        X_pre(:,i) = X_aekf(:,i-1)+X_next;
+        dk = Z - H*X_pre(:,i);
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = JF*P0*JF'+Q;
+        Kg = P*H'*inv(H*P*H'+R);
+        KK(1,1) = Kg(1,1);
+        KK(2,2) = Kg(2,2);
+        KK(4,3) = Kg(4,3);
+        X_aekf(:,i) = X_pre(:,i)+KK*(Z-H*X_pre(:,i));
+        epz = Z-H*X_aekf(:,i);
+        R=alfa*R+(1-alfa)*(epz*epz'+H*P*H');
+        Q=alfa*Q+(1-alfa)*Kg*dk*dk'*Kg';
+        P0 = (I-Kg*H)*P;
+    end
+    errAEKf(i) = norm(X_aekf(1:2,i)-[xt(i);yt(i)]);
+    theta_aekf(i) = X_aekf(4,i);
+    % if errAEKf(i)>15
+    %     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);
+figure;
+plot(x_imu,y_imu,xt,yt);
+mean(errAEKf_sw)
+
+
+
diff --git a/Code/Matlab/AOA-IMU/script_ekf.m b/Code/Matlab/AOA-IMU/script_ekf.m
new file mode 100644
index 0000000..bc8a612
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/script_ekf.m
@@ -0,0 +1,342 @@
+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 w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    x_imu(i) = x_imu(i-1)+vtx; 
+    y_imu(i) = y_imu(i-1)+vty;
+    errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
+    errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
+    X_next = [vtx;vty;detV;detTheta;detW];
+    if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
+        X_ekf(:,i) = X_ekf(:,i-1)+X_next;
+        errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
+        theta_ekf(i) = X_ekf(4,i);
+        continue
+    end    
+    if w<WW
+        X_pre(:,i) = X_ekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = F*P0*F'+Q1;
+        Kg = P*H'*inv(H*P*H'+R);
+        X_ekf(:,i) = X_pre(:,i)+Kg*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    else
+        JF = [1 0 1/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) v/w*(cosd(theta_imu(i))-cosd(theta_imu(i-1))) detD/w*cosd(theta_imu(i))-v/(w^2)*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+             0 1 1/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1))) v/w*(sind(theta_imu(i))-sind(theta_imu(i-1))) detD/w*sind(theta_imu(i))-v/(w^2)*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));
+             0 0 1 0 0;
+             0 0 0 1 detImuTime;
+             0 0 0 0 1];
+
+        X_pre(:,i) = X_ekf(:,i-1)+X_next;
+        Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+        P = JF*P0*JF'+Q1;
+        Kg = P*H'*inv(H*P*H'+R);
+        X_ekf(:,i) = X_pre(:,i)+Kg*(Z-H*X_pre(:,i));
+        P0 = (I-Kg*H)*P;
+    end
+    errEKf(i) = norm(X_ekf(1:2,i)-[xt(i);yt(i)]);
+    theta_ekf(i) = X_ekf(4,i);
+end
+%%  UKF
+% UKF settings
+ukf_L = 5; %numer of states
+ukf_m = 3; %numer of measurements
+ukf_kappa = 3 - ukf_L;
+ukf_alpha = 0.9;
+ukf_beta = 2;
+
+ukf_lambda = ukf_alpha^2*(ukf_L + ukf_kappa) - ukf_L;
+ukf_gamma = sqrt(ukf_L + ukf_lambda);
+ukf_W0_c = ukf_lambda / (ukf_L + ukf_lambda) + (1 - ukf_alpha^2 + ukf_beta);
+ukf_W0_m = ukf_lambda / (ukf_L + ukf_lambda);
+ukf_Wi_m = 1 / (2*(ukf_L + ukf_lambda));
+ukf_Wi_c = ukf_Wi_m;
+
+q=0.01;    %std of process 
+r=5;    %std of measurement
+p=2;
+Qu=q*eye(ukf_L); % std matrix of process
+Ru=r*eye(ukf_m);        % std of measurement 
+Pu=p*eye(ukf_L);
+H = [1 0 0 0 0;
+     0 1 0 0 0;
+     0 0 0 1 0];
+X_ukf(:,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 w<WW
+        vtx = detD*cosd(theta_imu(i));
+        vty = detD*sind(theta_imu(i));
+    else
+        vtx = v/w*(sind(theta_imu(i))-sind(theta_imu(i-1)));
+        vty = v/w*(-cosd(theta_imu(i))+cosd(theta_imu(i-1)));           
+    end
+    x_imu(i) = x_imu(i-1)+vtx; 
+    y_imu(i) = y_imu(i-1)+vty;
+    errUwb(i) = norm([x_uwb(i);y_uwb(i)]-[xt(i);yt(i)]);
+    errImu(i) = norm([x_imu(i);y_imu(i)]-[xt(i);yt(i)]);
+    X_next = [vtx;vty;detV;detTheta;detW];
+    if (x_uwb(i) == x_uwb(i-1))&&(y_uwb(i) == y_uwb(i-1))
+        X_ukf(:,i) = X_ukf(:,i-1)+X_next;
+        errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
+        theta_ukf(i) = X_ukf(4,i);
+        continue
+    end    
+
+    xestimate = X_ukf(:,i-1);
+    Xx = repmat(xestimate, 1, length(xestimate));
+    Xsigma = [xestimate, ( Xx + ukf_gamma * Pu ), ( Xx - ukf_gamma * Pu )];
+    
+    %第二步：对Sigma点集进行一步预测
+    [Xsigmapre]=fun1(Xsigma,detImuTime);
+    %第三步：估计预测状态
+    Xpred = ukf_W0_m * Xsigmapre(:,1) + ukf_Wi_m * sum(Xsigmapre(:,2:end), 2);
+    %第四步：均值和方差
+      Xx = repmat(Xpred, 1, length(xestimate)*2);
+     [~, R] = qr([sqrt(ukf_Wi_c) * ( Xsigmapre(:,2:end) - Xx ), Qu]', 0);
+     Ppred = cholupdate(R, sqrt(ukf_W0_c) * (Xsigmapre(:,1) - Xpred), '-')';
+
+    %第5步：根据预测值，再一次使用UT变换，得到新的sigma点集
+    Xx = repmat(Xpred, 1, length(xestimate));
+    Xsigmapre = [Xpred, ( Xx + ukf_gamma * Ppred ), ( Xx - ukf_gamma * Ppred )];
+
+    %第6步：观测预测
+    Zsigmapre=H*Xsigmapre;
+
+    %第7步：计算观测预测均值和协方差
+    Zpred = ukf_W0_m * Zsigmapre(:,1) + ukf_Wi_m * sum(Zsigmapre(:,2:end), 2);
+
+    Yy = repmat(Zpred, 1, length(xestimate)*2);
+    [~, Rz] = qr([sqrt(ukf_Wi_c) * (Zsigmapre(:,2:end) - Yy), Ru]', 0);
+    Pzz = cholupdate(Rz, sqrt(ukf_W0_c) * (Zsigmapre(:,1) - Zpred), '-')';
+
+    Xd = Xsigmapre - repmat(Xpred, 1, length(xestimate)*2 + 1);
+    Zd = Zsigmapre - repmat(Zpred, 1, length(xestimate)*2 + 1);
+  
+    Pxz = ( ukf_W0_c*  Xd(:,1) * Zd(:,1)' ) + ( ukf_Wi_c * Xd(:,2:end) * Zd(:,2:end)' );
+
+    %第七步：计算kalman增益
+    Kukf= (Pxz / Pzz') / Pzz;
+    %第八步：状态和方差更新
+    Z = [x_uwb(i);y_uwb(i);theta_uwb(i)];
+    Xpred=Xpred+Kukf*(Z-Zpred);
+    U = Kukf * Pzz;
+    Rp = Ppred';
+    for ii=1:size(U, 2)
+        Rp = cholupdate(Rp, U(:,ii), '-');
+    end
+    Ppred = Rp';
+    X_ukf(:,i)=Xpred;
+
+    errUKf(i) = norm(X_ukf(1:2,i)-[xt(i);yt(i)]);
+    theta_ukf(i) = X_ukf(4,i);
+end
+plot(errUKf);
+mean(errUKf)
\ No newline at end of file
diff --git a/Code/Matlab/AOA-IMU/script_kf.m b/Code/Matlab/AOA-IMU/script_kf.m
new file mode 100644
index 0000000..d74f033
--- /dev/null
+++ b/Code/Matlab/AOA-IMU/script_kf.m
@@ -0,0 +1,155 @@
+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.005;
+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;
+    % 观测矩阵: UWB的位置、UWB的航向角
+    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
+plot(errKf);
+mean(errKf)
\ No newline at end of file