UWBIns/Code/Matlab/AOA-IMU/script_kf.m

clear
clc
close all
load('data1.mat');
nn = size(imuPosX,1);
%% lightHouse<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<EFBFBD><EFBFBD>
x =  lightHousePosX * 100;
y = -lightHousePosZ * 100;

% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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));

% ʹ<EFBFBD><EFBFBD> fminsearch <EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ǻ<EFBFBD>λ<EFBFBD><EFBFBD>
initialGuess = [0, 10, 10]; % <EFBFBD><EFBFBD>ʼ<EFBFBD>²<EFBFBD>ֵ<EFBFBD><EFBFBD>[<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>, λ<EFBFBD><EFBFBD>]
optimizedParams = fminsearch(errorFunction, initialGuess);

% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ǻ<EFBFBD>λ<EFBFBD><EFBFBD>
rotationAngle = optimizedParams(1);
xOffset = optimizedParams(2);
yOffset = optimizedParams(3);
% <EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
xt = x * cos(rotationAngle) - y * sin(rotationAngle) + xOffset;
yt = x * sin(rotationAngle) + y * cos(rotationAngle) + yOffset;
%%
tagN = 4;
%<EFBFBD><EFBFBD>ǩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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));%Э<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;


% <EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>У׼
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
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>IMU<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̼Ƶ<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
    detImuTime = imuDataRxTime(i) - imuDataRxTime(i-1);
    detOdomTime = odomDataRxTime(i)-odomDataRxTime(i-1);
    % <EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD>ʱZ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
    w = wZ(i);
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<EFBFBD>ٶ<EFBFBD>
    v = vXY(i)*100;
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    detV=(vXY(i)-vXY(i-1))*100;
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    detD = v*detImuTime; 
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    detTheta = w*180/pi*detImuTime;
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵı仯<EFBFBD><EFBFBD>
    detW = w-wZ(i-1);
    % <EFBFBD><EFBFBD><EFBFBD>ּ<EFBFBD><EFBFBD><EFBFBD>IMU<EFBFBD>ĺ<EFBFBD>λ<EFBFBD><EFBFBD>
    theta_imu(i) = theta_imu(i-1)+detTheta;
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD>
    theta_imu(i) = mod(theta_imu(i)+180,360)-180;
    
    % ״̬<EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD>
    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;];
    % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˮƽ<EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    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;
    % <EFBFBD>۲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: UWB<EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD>UWB<EFBFBD>ĺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    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)