1.算法描述

六自由度四轴飞行器,包括由四根杆组成的正四面体,所述正四面体的中心位置设有一个空心圆球,空心圆球上设有四根支杆分别与正四面体的四个顶点相连,所述空心圆球内设有电池和控制系统,

INS/GPS的松组合模型所使用的传感器有三轴IMU，三轴磁强计和GPS接收机模块，它们分别提供载体在机体坐标系下的加速度信息、角速度信息、航向信息（已经预先基于三轴磁强计解算出航向信息，并减去当地磁偏角）、载体在导航系中的三维位置信息和导航系中的三维速度信息。各个传感器（尤其是IMU和三轴磁强计）需要经过初始校准，GPS接收机模块（以ublox m8n中ubx协议中的pvt为例）配置输出经度、纬度、海拔高度和NED坐标系下的三轴速度。

卡尔曼滤波（Kalman Filter）是一种高效的自回归滤波器，它能在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具。由鲁道夫.E.卡尔曼于1961年前后首次提出，并首先应用于阿波罗计划的轨道预测。

ekf

对于KF、EKF、UKF的主要差异，通俗一点说，主要如下：

卡尔曼滤波器 KF 是一种用作最小二乘误差优化器的滤波器，为了使其工作，在滤波器内部考虑的系统必须是线性的,即基于线性系统的前提假设。

为了使用KF对非线性系统进行状态估计或参数估计，一种可能的方法是围绕其当前状态对正在研究的系统进行线性化，并强制滤波器使用系统的这个线性化版本作为模型. 这是Extended Kalman Filter，或简称为EKF。

然而，由于EKF 不是很稳定，而且很多时候，当它确实收敛到“正确”解时，其收敛过程会非常缓慢。为了改进这个滤波器的固有不足，一些作者开始使用 Unscented Transformation，而不是使用线性化来预测被研究系统的行为。因此，具有 Unscented 变换（无迹变换）的 Kalman 滤波器称为 Unscented Kalman Filter，或简称为 UKF。

与 EKF 相比，UKF具有一些优势，因为 Unscented 变换在某种程度上比线性化更好地描述了非线性系统，因此该滤波器更迅速地收敛到正确的解。而使用 EKF，这个滤波器可能会变得不稳定，结果可能会出现偏差。

核心差异总结如下：
KF：线性化假设，求解线性系统，对应的是线性模型
EKF：求解对象是非线性系统，通过Taylor展开式，舍去高阶项，基于线性化思想来近似线性化
————————————————

INS

惯性导航系统（INS，以下简称惯导）是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

惯性导航系统（INS，Inertial Navigation System）也称作惯性参考系统，是一种不依赖于外部信息、也不向外部辐射能量（如无线电导航那样）的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

惯性导航系统属于推算导航方式，即从一已知点的位置根据连续测得的运动体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。惯性导航系统中的陀螺仪用来形成一个导航坐标系，使加速度计的测量轴稳定在该坐标系中，并给出航向和姿态角；加速度计用来测量运动体的加速度，经过对时间的一次积分得到速度，速度再经过对时间的一次积分即可得到位移。

2.仿真效果预览

matlab2022a仿真结果如下：

3.MATLAB核心程序

 
% IMU location specifier
r_imu=[-.5/12 -3/12 1/12]'*0; % Currently set to 0. Offset effects are not currently considered
r_GPS=[1.5, 0 ,0 ]; % This is the location of the GPS wrt CG, this is very important
%rotation matrix ------------------------------------------------------
phi= x(1);
theta= x(2);
psi = x(3);
 
 
L_bi = [cos(theta)*cos(psi) cos(theta)*sin(psi) -sin(theta); ...
    sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)  sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) sin(phi)*cos(theta); ...
    cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)  cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi) cos(phi)*cos(theta)];
 
 
Rt2b=L_bi';
 
[U,S,V]=svd(Rt2b);
R = U*V';
if 1+R(1,1)+R(2,2)+R(3,3) > 0
    b(1,1)    = 0.5*sqrt(1+R(1,1)+R(2,2)+R(3,3));
    b(2,1)    = ((R(3,2)-R(2,3))/4)/b(1);
    b(3,1)    = (R(1,3)-R(3,1))/4/b(1);
    b(4,1)    = (R(2,1)-R(1,2))/4/b(1);
    b       = b/norm(b);    
else
    R
    error('R diagonal too negative.')
    b = zeros(4,1);
end
 
 
%-----------------------------------------------------------------
q1=b(1);%the quaternions are called b1-b4 in the data file that you loaded
q2=b(2);
q3=b(3);
q4=b(4);
 
%initialize velocity
vx = 0;
vy = 0;
vz = 0;
 
%set sample time
dt = .02;
tf=size(A,2);
 
 
 
xhat = [0 0 0 0 0 0 b(1) b(2) b(3) b(4) bp bq br bfx bfy bfz]';
 
% noise params process noise 
Q = diag([.1 .1 .1 .1 .1 .1 .8 .8 .8 .8 .0001 .0001 .0001 .0001 .0001 .0001]);
 
 
R = diag([9 9 9 3 3 3]);
 
 
P = diag([30 30 30 3 3 3 .1 .1 .1 .1 .1 .1 .1 .1 .1 .1]);
Pdot=P*0;
tic
for k = 1:tf
    time= (k-1)*dt;
 
    xhatold=xhat;
    p = (A(1,k)*pi/180-xhat(11));
    q = (A(2,k)*pi/180-xhat(12));
    r = A(3,k)*pi/180-xhat(13);
    fx = (A(4,k)-xhat(14));
    fy = (A(5,k)-xhat(15));
    fz = -A(6,k)-xhat(16);  
    
    
    
    % Raw sensor measurments for plotting
    p_raw = A(1,k)*pi/180;
    q_raw = A(2,k)*pi/180;
    r_raw = A(3,k)*pi/180;
    fx_raw = A(4,k);
    fy_raw = A(5,k);
    fz_raw = A(6,k);
    
    quat = [xhat(7) xhat(8) xhat(9) xhat(10)]';
    quatmag= sqrt(quat(1)^2+quat(2)^2+quat(3)^2+quat(4)^2);
    
    quat = quat/quatmag;
 
    q1 = quat(1);
    q2 = quat(2);
    q3 = quat(3);
    q4 = quat(4);
 
    L_bl = [q1^2+q2^2-q3^2-q4^2    2*(q2*q3+q1*q4)      2*(q2*q4-q1*q3)
        2*(q2*q3-q1*q4)    q1^2-q2^2+q3^2-q4^2    2*(q3*q4+q1*q2)
        2*(q2*q4+q1*q3)      2*(q3*q4-q1*q2)    q1^2-q2^2-q3^2+q4^2];
    L_lb = L_bl';
    
    
 
    vq_term_11=2*(q1*fx-q4*fy+q3*fz);
    vq_term_12=2*(q2*fx+q3*fy+q4*fz);
    vq_term_13=2*(-q3*fx+q2*fy+q1*fz);
    vq_term_14=2*(-q4*fx-q1*fy+q2*fz);
    
    vq_term_21=2*(q4*fx+q1*fy-q2*fz);
    vq_term_22=2*(q3*fx-q2*fy-q1*fz);
    vq_term_23=2*(q2*fx+q3*fy+q4*fz);
    vq_term_24=2*(q1*fx-q4*fy+q3*fz);
    
    vq_term_31=2*(-q3*fx+q2*fy+q1*fz);
    vq_term_32=2*(q4*fx+q1*fy-q2*fz);
    vq_term_33=2*(-q1*fx+q4*fy-q3*fz);
    vq_term_34=2*(q2*fx+q3*fy+q4*fz);
    
    delv_delq=[[vq_term_11 vq_term_12 vq_term_13 vq_term_14];[vq_term_21 vq_term_22 vq_term_23 vq_term_24];[vq_term_31 vq_term_32 vq_term_33 vq_term_34]];
    
    %delV/del_abias
    delv_delba=(-1)*(L_lb);
    
    %delQ/delQ
    delq_delq=(-0.5)*[[0 p q r];[-p 0 -r q];[-q r 0 -p];[-r -q p 0]];
    
 
    %delQ/del_gyrobias
    delq_delbw=0.5*([[q2 q3 q4];[-q1 q4 -q3];[-q4 -q1 q2];[q3 -q2 -q1]]);
     
      
    % 现在组装过渡矩阵
    I=eye(3,3);
    trans_row1=[zeros(3,3) I zeros(3,4) zeros(3,3) zeros(3,3)];
    trans_row2=[zeros(3,3) zeros(3,3) delv_delq zeros(3,3) delv_delba];
    trans_row3=[zeros(4,3) zeros(4,3) delq_delq delq_delbw zeros(4,3)];
    trans_row4=[zeros(3,3) zeros(3,3) zeros(3,4) zeros(3,3) zeros(3,3)];
    trans_row5=[zeros(3,3) zeros(3,3) zeros(3,4) zeros(3,3) zeros(3,3)];
    
    phi_matrix=[trans_row1;trans_row2;trans_row3;trans_row4;trans_row5];
    
    phi_matrix_for_predict=phi_matrix(1:10,1:10);
   
    %下一状态预测
    xhat(1:10)=(expm(phi_matrix_for_predict*dt))*xhat(1:10)+[zeros(1,5) 32.2*dt zeros(1,4)]';
    quat_update=[xhat(7) xhat(8) xhat(9) xhat(10)]';
    quat_update_norm=norm(quat_update);
    xhat(7)=xhat(7)/quat_update_norm;
    xhat(8)=xhat(8)/quat_update_norm;
    xhat(9)=xhat(9)/quat_update_norm;
    xhat(10)=xhat(10)/quat_update_norm;
    
   %传播误差协方差矩阵，我建议使用连续积分，因为Q，R没有离散化
    Pdot=phi_matrix*P+P*phi_matrix'+Q;
    P1=Pdot*dt;
    P=P+P1;
    
    %% Correction step
    % 从GPS获取您的测量值、3个位置和3个速度
    z =[ A(7,k) A(8,k) A(9,k) A(10,k) A(11,k) A(12,k)]'; % x y z vx vy vz
       
    % Write out the measurement matrix linearization to get H
    
    %del P/del q
    Hxq_row_1=[-r_GPS(1)*2*q1 -r_GPS(1)*2*q2 r_GPS(1)*2*q3 r_GPS(1)*2*q4];
    Hxq_row_2=[-r_GPS(1)*2*q4 -r_GPS(1)*2*q3 -r_GPS(1)*2*q2 -r_GPS(1)*2*q1];
    Hxq_row_3=[r_GPS(1)*2*q3 -r_GPS(1)*2*q4 r_GPS(1)*2*q1 -r_GPS(1)*2*q2];
    H_delp_delq=[Hxq_row_1;Hxq_row_2;Hxq_row_3];
    
    % del v/del q
    
    Hvq_row_1=[(r_GPS(1)*2*q3*q+r_GPS(1)*2*q4*r) (r_GPS(1)*2*q4*q-r_GPS(1)*2*q3*r) (r_GPS(1)*2*q1*q-r_GPS(1)*2*q2*r) (r_GPS(1)*2*q2*q+r_GPS(1)*2*q1*r)];
    Hvq_row_2=[(-r_GPS(1)*2*q2*q-r_GPS(1)*2*q1*r) (r_GPS(1)*2*q2*r-r_GPS(1)*2*q1*q) (r_GPS(1)*2*q4*q-r_GPS(1)*2*q3*r) (r_GPS(1)*2*q3*q+r_GPS(1)*2*q4*r)];
    Hvq_row_3=[(r_GPS(1)*2*q1*q-r_GPS(1)*2*q2*r) (-r_GPS(1)*2*q2*q-r_GPS(1)*2*q1*r) (-r_GPS(1)*2*q3*q-r_GPS(1)*2*q4*r) (r_GPS(1)*2*q4*q-r_GPS(1)*2*q3*r)];
    H_delv_delq=[Hvq_row_1;Hvq_row_2;Hvq_row_3];
    
    % Assemble H
    H=[[I zeros(3,3) H_delp_delq zeros(3,6)];[zeros(3,3) I H_delv_delq zeros(3,6)]];
    %rank(obsv(phi_matrix,H))
    %Compute Kalman gain
    S=H*P*H'+R;
    K=P*H'*inv(S);
 
    xhat=xhat+K*(z-H*xhat);
    
    P=(eye(16,16)-K*H)*P;
 
    quatprev=[xhatold(7) xhatold(8) xhatold(9) xhatold(10)]';
    quatprev=quatprev/norm(quatprev);
     
 
    [phi(k),theta(k),psi(k)]=quat2euler(quatprev');
    phi(k)=phi(k)*180/pi;
    theta(k)=theta(k)*180/pi;
    psi(k)=psi(k)*180/pi;
    
    quat1 = A(13:16,k);
    [phi_raw(k),theta_raw(k),psi_raw(k)]=quat2euler(quat1');
    phi_raw(k)=phi_raw(k)*180/pi;
    theta_raw(k)=theta_raw(k)*180/pi;
    psi_raw(k)=psi_raw(k)*180/pi;
    xhatR(k,:)= [xhatold(1:6)' quatprev(1) quatprev(2) quatprev(3) quatprev(4) xhatold(11:16)'];
    P_R(k,:) = diag(P);
    z_R(k,:) = z;
    OMEGA_raw(k,:)=[p_raw,q_raw,r_raw]';
    OMEGA(k,:)=[p,q,r]';
    FX(k,:)=[fx_raw,fy_raw,fz_raw]';
  
end
toc
t = 1:(k);
A124