基于kalman滤波的gps组合导航滤波算法.rar_GPS_GPS Kalman滤波_kalman滤波 GPS_滤波_滤波算法

% SINS/GPS组合导航 clc; clearvars; %% 参数初始化 %固定参数 re=6378137; %地球长半径Re（公里） e=1/298.257; %椭圆度e wie=7.292115e-5; %地球自转角速度 dtor=pi/180; %角度转换成弧度 rtod=180/pi; g0=9.7803267714; %论文中给出的g0 dt=0.01; %IMU输出数据频率100Hz total_t=3600; %总仿真时间3600秒 N=total_t/dt; %仿真数据数 t=0:dt:total_t; %时间序列 %初始位置、姿态参数设置 Lati_initial=34.652160625*dtor; %初始纬度（弧度） Long_initial=109.249623717*dtor; %初始经度（弧度） Height_initial=362.2690; %初始高度（米） fi_initial=303.10881*dtor; %初始航向角（弧度） th_initial=0.25516*dtor; %初始俯仰角（弧度） gm_initial=1.76037*dtor; %初始横滚角（弧度） g=g0*(1-2*Height_initial/re)*(1+0.00193185138639*(sin(Lati_initial))^2)/sqrt(1-0.00669437999013*(sin(Lati_initial))^2); %WGS-84全球大地坐标系下重力加速度 ug=1e-6*g; %定义ug q=[cos(fi_initial/2)*cos(th_initial/2)*cos(gm_initial/2)-sin(fi_initial/2)*sin(th_initial/2)*sin(gm_initial/2); cos(fi_initial/2)*sin(th_initial/2)*cos(th_initial/2)-sin(fi_initial/2)*cos(th_initial/2)*sin(gm_initial/2); cos(fi_initial/2)*cos(th_initial/2)*sin(th_initial/2)+sin(fi_initial/2)*sin(th_initial/2)*cos(gm_initial/2); cos(fi_initial/2)*sin(th_initial/2)*sin(th_initial/2)+sin(fi_initial/2)*cos(th_initial/2)*cos(gm_initial/2)]; %初始四元数 Cnb=[cos(gm_initial)*cos(fi_initial)-sin(gm_initial)*sin(th_initial)*sin(fi_initial),cos(gm_initial)*sin(fi_initial)+sin(gm_initial)*sin(th_initial)*cos(fi_initial),-sin(gm_initial)*cos(th_initial); -cos(th_initial)*sin(fi_initial),cos(th_initial)*cos(fi_initial),sin(th_initial); sin(gm_initial)*cos(fi_initial)+cos(gm_initial)*sin(th_initial)*sin(fi_initial),sin(gm_initial)*sin(fi_initial)-cos(gm_initial)*sin(th_initial)*cos(fi_initial),cos(gm_initial)*cos(th_initial)]; %初始姿态方向余弦矩阵 %Cen=[1,0,0;0,sin(Lati_initial),cos(Lati_initial);0,-cos(Lati_initial),sin(Lati_initial)]*[-sin(Long_initial),cos(Long_initial),0;-cos(Long_initial),-sin(Long_initial),0;0,0,1]; %初始位置方向余弦矩阵 %组合导航系统误差参数设置 [err_hor,err_height,err_v,accp,accn,gyrop,gyron,err_fi,err_th,err_gm]=para_init(); %卡尔曼滤波初始参数 %状态变量x：数学平台失准角（东北天）、速度误差（东北天）、位置误差（纬经高）、陀螺常值漂移（xyz）、加计常值偏置（xyz）(15维) %过程噪声w：陀螺随机误差（xyz）、加计随机误差（xyz） %量测变量y：捷联解算与GPS之差（东向速度、北向速度、天向速度、纬度、经度、高度） %量测噪声v：根据GPS位置、速度噪声水平选取 dtfil=0.05; %滤波周期为0.05s,也是GPS周期 bl=dtfil/dt; tkalm=0:dtfil:total_t; %卡尔曼滤波时间序列 err_v_init=0.02; %P(0)中初始速度误差 err_l_init=0.1; %P(0)中初始位置误差0.1m kalmp=diag([err_th^2,err_gm^2,err_fi^2,err_v_init^2,err_v_init^2,err_v_init^2,(err_l_init/re)^2,(err_l_init/re)^2,err_l_init^2,gyrop^2,gyrop^2,gyrop^2,accp^2,accp^2,accp^2]); %卡尔曼滤波P(0),第四五六项初始速度误差设置为0.02m/s，第七八项初始位置误差为0.1弧度/re，第九项初始高度误差为0.1米 kalmp_initial=kalmp; kalmq=diag([gyron^2,gyron^2,gyron^2,accn^2,accn^2,accn^2]); %连续情况下卡尔曼滤波系统噪声方差矩阵Q（6*6） kalmr=diag([err_v^2,err_v^2,err_v^2,(err_hor/(re+Height_initial))^2,(err_hor/(re+Height_initial)/cos(Lati_initial))^2,err_height^2]); %离散情况下卡尔曼滤波测量噪声方差矩阵R（6*6） kalmx=zeros(15,1); %卡尔曼滤波x(0) %轨迹发生器给出数据 fb=zeros(3,N); %加计输出数据 wibb=zeros(3,N); %陀螺输出数据 GPS_velo=zeros(3,N/bl); %GPS理论输出速度数据，GPS输出频率为20Hz GPS_loca=zeros(3,N/bl); %GPS理论输出位置数据，GPS输出数据为20Hz %待计算变量 v=zeros(3,N+1); %速度变量（第一行为东向速度，第二行为北向速度，第三行为天向速度），初始速度赋值在读入数据部分 v_part=zeros(3,N/bl); %部分速度滤波结果，与track时间节点相对应 fi(1,N)=0; %航向角计算结果 th(1,N)=0; %俯仰角计算结果 gm(1,N)=0; %横滚角计算结果 fi_part=zeros(1,N/bl); th_part=zeros(1,N/bl); gm_part=zeros(1,N/bl); %部分姿态角滤波结果，与track时间节点相对应 Lati=[Lati_initial,zeros(1,N)]; %纬度变量（第一个为初始纬度） Long=[Long_initial,zeros(1,N)]; %经度变量（第一个为初始经度） Height=Height_initial*ones(1,N+1); %高度变量,设高度不变（第一个为初始高度） Lati_part=zeros(1,N/bl); Long_part=zeros(1,N/bl); Height_part=zeros(1,N/bl); %部分位置滤波结果，与track时间节点相对应 x_kf(15,N/bl) = 0; %状态变量x p_kf(15,N/bl) = 0; %估计误差方差P（记录P对角线元素的开方） %% 数据读取 IMU=load('IMU.mat'); fb=IMU.IMU(2:N+1,6:8)'*g; %单位米/秒^2 wibb=IMU.IMU(2:N+1,3:5)'*dtor/3600; %单位弧度/秒 GPS=load('GPS.mat'); v(:,1)=GPS.GPS(1,6:8)'; GPS_velo=GPS.GPS(2:N/bl+1,6:8)'; %速度单位米/秒 GPS_loca(1:2,:)=GPS.GPS(2:N/bl+1,3:4)'*dtor; %经纬度用弧度表示 GPS_loca(3,:)=GPS.GPS(2:N/bl+1,5)'; %高度单位为米 %% SINS/GPS组合导航和卡尔曼滤波 for i=1:1:N %———————————四元数、姿态矩阵和姿态角更新—————————— rmh=re*(1-2*e+3*e*(sin(Lati(i)))^2)+Height(i); %沿子午圈主曲率半径 rnh=re*(1+e*(sin(Lati(i)))^2)+Height(i); %沿卯酉圈主曲率半径 wien=[0 wie*cos(Lati(i)) wie*sin(Lati(i))]'; wenn=[-v(2,i)/rmh v(1,i)/rnh v(1,i)*tan(Lati(i))/rnh]'; winn=wien+wenn; winb=Cnb*winn; wnbb=wibb(:,i)-winb; %姿态速率 change_angle = wnbb * dt; %角度改变量 M = [0, -change_angle(1), -change_angle(2), -change_angle(3); change_angle(1), 0, change_angle(3), -change_angle(2); change_angle(2), -change_angle(3), 0, change_angle(1); change_angle(3), change_angle(2), -change_angle(1), 0]; delta_theta0=sqrt(M(1,2)^2+M(1,3)^2+M(1,4)^2); q=(cos(delta_theta0/2)*eye(4) + sin(delta_theta0/2)/delta_theta0*M)*q; q_det=sqrt(q(1,1)^2+q(2,1)^2+q(3,1)^2+q(4,1)^2); q=q/q_det; %四元数单位化 论文中采用四阶离散化方法，这里直接采用三角函数形式 Cnb = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4)), 2*(q(2)*q(4)-q(1)*q(3)); 2*(q(2)*q(3)-q(1)*q(4)), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(1)*q(2)); 2*(q(2)*q(4)+q(1)*q(3)), 2*(q(3)*q(4)-q(1)*q(2)), q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4)]; % [fi(i),th(i),gm(i)]=attitude(Cnb); %三个姿态角更新，输出结果为弧度制 %——————————————end————————————————— %———————————速度更新（东北天）—————————————— g=g0*(1-2*Height(i)/re)*(1+0.00193185138639*(sin(Lati(i)))^2)/sqrt(1-0.00669437999013*(sin(Lati(i)))^2); %WGS-84全球大地坐标系下重力加速度 ug=1e-6*g; %定义ug fb_n=Cnb'*fb(:,i); %加速度计在导航系下值 at_E=fb_n(1)+(2*wien(3)+wenn(3))*v(2,i)-(2*wien(2)+wenn(2))*v(3,i); at_N=fb_n(2)-(2*wien(3)+wenn(3))*v(1,i)+(2*wien(1)+wenn(1))*v(3,i); at_U=fb_n(3)+(2*wien(2)+wenn(2))*v(1,i)-(2*wien(1)+wenn(1))*v(2,i)-g; % at_E=fb_n(1)+(2*wie*sin(Lati(i))+v(1,i)*tan(Lati(i))/(rn+Height(i)))*v(2,i)-(2*wie*cos(Lati(i))+v(1,i)/(rn+Height(i)))*v(3,i); % at_N=fb_n(2)-(2*wie*sin(Lati(i))+v(1,i)*tan(Lati(i))/(rn+Height(i)))*v(1,i)-v(2,i)/(rm+Height(i))*v(3,i); % at_U=fb_n(3)+(2*wie*cos(Lati(i))+v(1,i)/(rn+Height(i)))*v(1,i)+v(2,i)/(rm+Height(i))*v(2,i)-g; v(1,i+1)=v(1,i)+at_E*dt; v(2,i+1)=v(2,i)+at_N*dt; v(3,i+1)=v(3,i)+at_U*dt; %——————————————end————————————————— %————————————位置更新——————————————— % det_wenn=[0,-wenn(3),wenn(2);wenn(3),0,-wenn(1);-wenn(2),wenn(1),0]; % Cen=Cen-dt*det_wenn*C