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) > 0b(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);
elseRerror('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:tftime= (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 plottingp_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_abiasdelv_delba=(-1)*(L_lb);%delQ/delQdelq_delq=(-0.5)*[[0 p q r];[-p 0 -r q];[-q r 0 -p];[-r -q p 0]];%delQ/del_gyrobiasdelq_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 qHxq_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 qHvq_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 HH=[[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 gainS=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

4.完整MATLAB

基于6自由度飞行器的EKF和INS融合算法的MATLAB仿真相关推荐

m基于深度学习的OFDM信道估计和均衡算法误码率matlab仿真,对比了LS,MMSE以及LMMSE等传统的信道估计算法
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述随着无线通信的快速发展,5G正逐渐成长为支撑全社会各行业运作的大型基础性互联网络,其服务范围的大幅扩 ...
【频谱共享】基于认知无线电的VCG拍卖机制频谱共享算法的MATLAB仿真
目录 1.软件版本 2.本算法理论知识点 3.算法具体理论 4.部分核心代码 5.仿真演示 6.本算法写论文思路 7.参考文献 8.相关算法课题及应用 1.软件版本 matlab2021a 2.本算法 ...
基于肤色空间建模+连通域处理的人脸检测算法的MATLAB仿真
目录 1.算法仿真效果 2.MATLAB核心程序 3.算法涉及理论知识概要 4.完整MATLAB 1.算法仿真效果 matlab2022a仿真结果如下: 2.MATLAB核心程序 .......... ...
什么是陀螺仪的dr算法_一种基于DR/GPS/MM的组合定位系统数据融合算法
摘要: 针对盲区中使用INS惯性导航系统进行定位存在误差积累的问题,提出一种基于DR航位推算.GPS全球定位系统和MM地图匹配的组合定位系统数据融合算法.该算法利用GPS和MM中得到的位置信息,一方 ...
基于暗通道优先的单幅图像去雾算法（Matlab）
基于暗通道优先的单幅图像去雾算法(Matlab) 每一幅图像的RGB三个颜色通道中,总有一个通道的灰度值很低,几乎趋向于0.基于这个几乎可以视作是定理的先验知识,作者提出暗通道先验的去雾算法. 首 ...
基于联邦学习的多源异构数据融合算法
摘要随着科技的迅猛发展,具有计算和存储能力的边缘设备数量不断增加,产生的数据流量更是呈指数式增长,这使得以云计算为核心的集中式处理模式难以高效处理边缘设备产生的数据.另外,由于边缘网络设备的多样性 ...
基于联邦学习的多源异构数据融合算法 Multi-Source Heterogeneous Data Fusion Based on Federated Learning
5.基于联邦学习的多源异构数据融合算法 Multi-Source Heterogeneous Data Fusion Based on Federated Learning 摘要:随着科技的迅猛发展, ...
m基于改进PSO粒子群优化的RBF神经网络解耦控制算法matlab仿真
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述智能控制的思想最早来自傅京孙教授[,他通过人机控制器和机器人方面的研究,首先把人工智能的自觉推理方法 ...
GD32F103+MPU9150四旋翼飞行器第一步：姿态融合算法
前言: 相比直升机来说,四旋翼乃至多旋翼飞行器的机械结构简单,操控灵活,飞行稳定,体积也能做的更小,当然也能更大,它将直升机复杂的机械结构设计难度转化到了电子电路和算法上面,因此四旋翼飞行器的设计更容 ...

基于6自由度飞行器的EKF和INS融合算法的MATLAB仿真

1.算法描述

2.仿真效果预览

3.MATLAB核心程序

4.完整MATLAB

基于6自由度飞行器的EKF和INS融合算法的MATLAB仿真相关推荐

最新文章

热门文章