eskf imu arhs

clear;clc;
%rng(100);
addpath(genpath(pwd));
load_param;
global params;
dt = params.dt;
[imudata,visdata] = vio_data_gen();
l_imu = length(imudata);

eskf.q = [1;0;0;0];
eskf.bg=[0;0;0];
eskf.g=[0;0;9.81];

eskf.R=diag(params.pix_sig.^2 *10* [1,1,1]); 
eskf.P = diag([0,0,0, (1e-3)^2 * [1,1,1]]);

    acc_N = params.acc_sig/sqrt(dt);
    acc_W = params.acc_bia*sqrt(dt);
    gyr_N = params.gyr_sig/sqrt(dt);
    gyr_W = params.gyr_bia*sqrt(dt);
    
eskf.Q =1* diag([gyr_N^2*[1,1,1].*dt^2, gyr_W^2*[1,1,1].*dt]);

angle_error=zeros(l_imu-1,3);

for i=1:l_imu-1
    t_imu = imudata(i).timestamp;
    t_imu_next = imudata(i+1).timestamp;
    %init
    
    if i==1
        eskf.q = qFromR(imudata(i).Rwb);
        continue;
    end    
    
    gyro1 = imudata(i).imu_gyro - eskf.bg;
    gyro2 = imudata(i+1).imu_gyro - eskf.bg;
    eskf.q =  RK4(eskf.q,gyro1,gyro2,dt);
    
    Rs = q2R(eskf.q);
    Ft = zeros(6);
    Ft(1:3,4:6) = -Rs;
   
    Fi =eye(6,6);
    IFT = eye(size(Ft)) + Ft.*dt;
    eskf.P =IFT* eskf.P *IFT' + Fi*eskf.Q *Fi';
    
     if 01 
       Gz = eskf.g(3);
       q0 = eskf.q(1);
       q1 = eskf.q(2);
       q2 = eskf.q(3);
       q3 = eskf.q(4);
       j1 = [ -2*Gz*q2,  2*Gz*q3, -2*Gz*q0, 2*Gz*q1, 0, 0, 0];
       j2 = [  2*Gz*q1,  2*Gz*q0,  2*Gz*q3, 2*Gz*q2, 0, 0, 0];
       j3 = [  2*Gz*q0, -2*Gz*q1, -2*Gz*q2, 2*Gz*q3, 0, 0, 0];
       Hx = [j1;j2;j3];
        
        
        Hq = 0.5*qRmat(eskf.q)*[0,0,0;eye(3)];
        Hy = zeros(7,6);
        Hy(1:4,1:3) = Hq;
        Hy(5:7,4:6) = eye(3);
       
        H = Hx*Hy;
       
        K = (eskf.P * H') / (eskf.R + H * eskf.P * H');
  
        ds = K*(imudata(i).imu_acc - Rs'*[0;0;eskf.g(3)]);
        KH = K*H;
        eskf.P = (eye(size(KH)) - KH)*eskf.P;
        
        eskf.q = qLmat(0.5.*[2;1*[ds(1);ds(2);ds(3)]]) * eskf.q;
        eskf.q = eskf.q/norm(eskf.q);
        eskf.bg = eskf.bg + ds(4:6);
        
        
        %plot3( eskf.p(1), eskf.p(2), eskf.p(3),'o');hold on;
        
        %reset 
        G = eye(6);
        G(1:3,1:3) = eye(3) + 0.5 *skew([ds(1);ds(2);ds(3)]);
        eskf.P = G * eskf.P *G';
        
        eskf.P = 0.5*(eskf.P + eskf.P');
    end
    [a1 a2 a3] =dcm2angle( imudata(i).Rwb);
    [a_1 a_2 a_3] = dcm2angle(q2R(eskf.q));
    res = [a1, a2, a3] - [a_1, a_2 ,a_3];

    if res(1)  <-pi
        res(1)  = res(1) +2*pi;
    elseif res(1) > pi
        res(1) = res(1) -2* pi;
    end

    angle_error(i,:)=  [ abs(res(1)), abs(res(2)), abs(res(3))]*180/pi;
end

plot(angle_error(:,1));hold on;
plot(angle_error(:,2));
plot(angle_error(:,3));grid on;
legend('a1','a2','a3');

 

 

function [eskf] = eskf_6_axis_arhs(eskf,acc,gyr,dt)
    %forward
    gyro1 = gyr - eskf.bg;
    eskf.q = qLmat(eskf.q)*[2;gyro1(1)*dt;gyro1(2)*dt;gyro1(3)*dt];
    eskf.q =  eskf.q/norm(eskf.q);
    Rs = q2R(eskf.q);
    Ft = zeros(6);
    Ft(1:3,4:6) = -Rs;
   
    Fi =eye(6,6);
    IFT = eye(size(Ft)) + Ft.*dt;
    eskf.P =IFT* eskf.P *IFT' + Fi*eskf.Q *Fi';
    %update
       Gz = 1;
       q0 = eskf.q(1);
       q1 = eskf.q(2);
       q2 = eskf.q(3);
       q3 = eskf.q(4);
       j1 = [ -2*Gz*q2,  2*Gz*q3, -2*Gz*q0, 2*Gz*q1, 0, 0, 0];
       j2 = [  2*Gz*q1,  2*Gz*q0,  2*Gz*q3, 2*Gz*q2, 0, 0, 0];
       j3 = [  2*Gz*q0, -2*Gz*q1, -2*Gz*q2, 2*Gz*q3, 0, 0, 0];
       Hx = [j1;j2;j3];
        
        Hq = 0.5*qRmat(eskf.q)*[0,0,0;eye(3)];
        Hy = zeros(7,6);
        Hy(1:4,1:3) = Hq;
        Hy(5:7,4:6) = eye(3);
       
        H = Hx*Hy;
       
        K = (eskf.P * H') / (eskf.R + H * eskf.P * H');
        
        acc_normal = acc/norm(acc);
        ds = K*(acc_normal - Rs'*[0;0;1]);
        KH = K*H;
        eskf.P = (eye(size(KH)) - KH)*eskf.P;
        
        eskf.q = qLmat([2;1*[ds(1);ds(2);ds(3)]]) * eskf.q;
        eskf.q = eskf.q/norm(eskf.q);
        eskf.bg = eskf.bg + ds(4:6);
        
        %plot3( eskf.p(1), eskf.p(2), eskf.p(3),'o');hold on
        %reset 
        G = eye(6);
        G(1:3,1:3) = eye(3) + 0.5 *skew([ds(1);ds(2);ds(3)]);
        eskf.P = G * eskf.P *G';
        eskf.P = 0.5*(eskf.P + eskf.P');

end

 

clear;clc;
%rng(100);
addpath(genpath(pwd));
load_param;
global params;
dt = params.dt;
[imudata,visdata] = vio_data_gen();
l_imu = length(imudata);

eskf.q = [1;0;0;0];
eskf.bg=[0;0;0];

eskf.R=diag(params.pix_sig.^2 *10* [1,1,1]); 
eskf.P = diag([0,0,0, (1e-3)^2 * [1,1,1]]);

    acc_N = params.acc_sig/sqrt(dt);
    acc_W = params.acc_bia*sqrt(dt);
    gyr_N = params.gyr_sig/sqrt(dt);
    gyr_W = params.gyr_bia*sqrt(dt);
    
eskf.Q =1* diag([gyr_N^2*[1,1,1].*dt^2, gyr_W^2*[1,1,1].*dt]);
angle_error=zeros(l_imu-1,3);

for i=1:l_imu-1
   eskf =  eskf_6_axis_arhs(eskf,imudata(i).imu_acc,imudata(i).imu_gyro,dt);
    [a1 a2 a3] =dcm2angle( imudata(i).Rwb);
    [a_1 a_2 a_3] = dcm2angle(q2R(eskf.q));
    res = [a1, a2, a3] - [a_1, a_2 ,a_3];
    if res(1)  <-pi
        res(1)  = res(1) +2*pi;
    elseif res(1) > pi
        res(1) = res(1) -2* pi;
    end
    angle_error(i,:)=  [ abs(res(1)), abs(res(2)), abs(res(3))]*180/pi;
end

plot(angle_error(:,1));hold on;
plot(angle_error(:,2));
plot(angle_error(:,3));grid on;
legend('a1','a2','a3');

return ;