function [delta_x,Vn_k1,pos_k1,q_bn_k1,b_g,b_a,s_g,s_a] = ...
    feedback_compensation(Vn,pos,q_bn,delta_x,bg,ba,sg,sa)
% [delta_x,Vn_k1,pos_k1,q_bn_k1,b_g,b_a,s_g,s_a] = ...
%    feedback_compensation(Vn,pos,q_bn,delta_x,bg,ba,sg,sa)
% feedback_compensation.m  使用误差状态向量对位置、姿态、和速度进行校正，
%                          并对陀螺零偏、加表零偏、陀螺比例因子、加表比
%                          例因子进行校正，最后将状态向量置零
%         input:
%            Vn   待校正的速度
%           pos   待校正的位置
%          q_bn   待校正的姿态四元素
%       delta_x   误差状态向量（21x1）
%            bg   陀螺零偏
%            ba   加表零偏
%            sg   gyro scale factor
%            sa   accelemeter scale factor
%       output:
%       delat_x   置零后的误差状态向量
%         Vn_k1   校正后的速度
%        pos_k1   校正后的位置
%       q_bn_k1   校正后的姿态
%------------dengchengjian--created in 20210715---------------------
%last modified:20210722

%% 位置校正
lat = pos(1);
lon = pos(2);
h   = pos(3);
RN  = R_N(lat);
RM  = R_M(lat);
delta_r_N = delta_x(1);  
delta_r_E = delta_x(2);

delta_theta = [delta_r_E/(RN+h), -delta_r_N/(RM+h), -delta_r_E*tan(lat)/(RN+h)]';
q_nc = Quat_BA(delta_theta);
q_ce    = [cos(-pi/4.0-0.5*lat)*cos(0.5*lon), ...
          -sin(-pi/4.0-0.5*lat)*sin(0.5*lon), ...
           sin(-pi/4.0-0.5*lat)*cos(0.5*lon), ...
           cos(-pi/4.0-0.5*lat)*sin(0.5*lon)];
q_ne = quat_multi(q_ce,q_nc);

pos_k1(1,1) = -2*atan(q_ne(3)/q_ne(1))-0.5*pi;
pos_k1(2,1) = 2*atan(q_ne(4)/q_ne(1));
pos_k1(3,1) = h+delta_x(3);

% D_inverse = diag([1/(RM+h), 1/(RN+h)/cos(lat), -1]);
% delta_pos = D_inverse*delta_x(1:3);
% pos_k1 = pos - delta_pos;

%% 速度校正

Vn_k1 = Vn - delta_x(4:6);

%% 姿态校正

% phi = delta_x(7:9); % 列向量
% q_pn = Quat_AB(phi);
% q_bn_k1 = quat_multi(q_pn,q_bn);

C_bp = quat2DCM(q_bn);
C_tp = eye(3)-SSM(delta_x(7:9));
C_bt = C_tp\C_bp;
[phi,theta,psi] = DCM2Euler(C_bt);
q_bn_k1 = Euler2quat(phi,theta,psi);

%% 零偏、比例因子累积
% b_g = bg+([1;1;1]+sg).*delta_x(10:12);
% b_a = ba+([1;1;1]+sa).*delta_x(13:15);
% s_g = sg+([1;1;1]+sg).*delta_x(16:18);
% s_a = sa+([1;1;1]+sa).*delta_x(19:21);
b_g = bg + delta_x(10:12);
b_a = ba + delta_x(13:15);
s_g = sg + delta_x(16:18);
s_a = sa + delta_x(19:21);
%% 误差状态向量置零
delta_x = zeros(21,1);

end