1.完整项目描述和程序获取
>面包多安全交易平台:https://mbd.pub/o/bread/Y5qVlJpv
>如果链接失效，可以直接打开本站店铺搜索相关店铺：
>如果链接失效，程序调试报错或者项目合作也可以加微信或者QQ联系。
2.部分仿真图预览
3.算法概述
六自由度四轴飞行器,包括由四根杆组成的正四面体,所述正四面体的中心位置设有一个空心圆球,空心圆球上设有四根支杆分别与正四面体的四个顶点相连,所述空心圆球内设有电池和控制系统,
INS/GPS的松组合模型所使用的传感器有三轴IMU，三轴磁强计和GPS接收机模块，它们分别提供载体在机体坐标系下的加速度信息、角速度信息、航向信息（已经预先基于三轴磁强计解算出航向信息，并减去当地磁偏角）、载体在导航系中的三维位置信息和导航系中的三维速度信息。各个传感器（尤其是IMU和三轴磁强计）需要经过初始校准，GPS接收机模块（以ublox m8n中ubx协议中的pvt为例）配置输出经度、纬度、海拔高度和NED坐标系下的三轴速度。
卡尔曼滤波（Kalman Filter）是一种高效的自回归滤波器，它能在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具。由鲁道夫.E.卡尔曼于1961年前后首次提出，并首先应用于阿波罗计划的轨道预测。
4.部分源码
%-----------------------------------------------------------------
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
