组合导航系统的计算程序代码

组合导航系统的计算程序代码

function yy=ukf_IMUgps()

%function ukf_IMUgps()

% UKF在IMU/GPS组合导航系统中应用

%

% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；

% 以GPS中的位置、速度为观测量。

%

% 7,July 2008.

clc

% Initialise state

global we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wa

d=0; %验证循环次数

%地球自转角速度we(rad/s):

we=7.292115e-5;

g=9.81; %地球重力加速度（m/s^2）

a=6.378137e+6; %地球长半轴

e2=0.; %地球第一偏心率的平方

%姿态角初始值（r,p,y）

zitai=(pi/180).*[0 2.0282 196.9087];

%姿态误差角

fai=(pi/180).*[1/36 1/36 5/36]; %(100'',100'',500'')

r=zitai(1)+fai(1);

p=zitai(2)+fai(2);

y=zitai(3)+fai(3);

%当地坐标系（l）相对于载体坐标系（b）的转换矩阵：Rbl(在e,n,u坐标系下）Rbl=[cos(r)*cos(y)-sin(r)*sin(y)*sin(p) -sin(y)*cos(p) cos(y)*sin(r)+sin(y)*sin(p)*cos(r) cos(r)*sin(y)+sin(r)*cos(y)*sin(p) cos(y)*cos(p) sin(y)*sin(r)-cos(y)*sin(p)*cos(r) -cos(p)*sin(r) sin(p) cos(p)*cos(r)];

%由转换矩阵Rbl计算四元数的初始值Q0=[q1,q2,q3,q4]'

=[0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3))

0.25*(Rbl(3,2)-Rbl(2,3))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))

0.25*(Rbl(1,3)-Rbl(3,1))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))

0.25*(Rbl(2,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];

%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,m,h）＝（经度，纬度,高度），姿态误差角fai(e,n,u)

vIMU=[-21.775011 -71.631027 3.10094];

point_IMU=[0. 0. 3122.826];

T=1; %UKF滤波的采样周期（s）

%陀螺常值漂移初始值tuo(e,n,u)(单位：。/s）

tuo=[0 0 0];

%陀螺一阶相关时间Tt(s)

Tt=[60 60 60];

%加速度计常值漂移初始值jiasu(e,n,u)(单位：m/s^2）

jiasu=[0 0 0];

%加速度计一阶反相关时间Ta(s)

Ta=[60 60 60];

%IMU系统的状态向量X

x=[vIMU point_IMU fai tuo jiasu]';

%观测量噪声V的斜方差矩阵

R=[];

%GPS系统的量测值Z(vn,ve,vd,m,l,h)

[z Rz]=gps_tiqushuju;

%Vk的噪声序列方差为：Rk

Rz=(Rz./T);

%陀螺常值漂移wt(e,n,u)

wt=(pi/180).*[1/(3600) 1/(3600) 1/(3600)]; % 1 (。/h)

%陀螺常值漂移误差wtt(e,n,u)

wtt=(pi/180).*[0.08/(3600) 0.08/(3600) 0.08/(3600)]; % 0.08 (。/h)

%加速度计常值漂移wa(e,n,u)

wa=[2e-6 2e-6 2e-6]; % 200μg (2e-6 m/s^2)

%加速度计常值漂移误差waa(e,n,u)

waa=[2e-6 2e-6 2e-6]./4; % 50μg (0.5e-6 m/s^2)

%姿态误差角噪声wg

wg=wt;

%状态向量X的斜方差矩阵

P = eye(length(x))*eps; % note: for stability, P should never be quite zero

%速度误差：（0.1m/s) 位置误差：水平（1m),高度（5m)

u=[0.1 0.1 0.1 0. 0. 2 wg wtt waa]';

%IMU系统在载体坐标系下的比力值输出值fb

fb=[];

%IMU系统中陀螺输出量

wib=[]; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量

[f w]=IMU_tiqushuju; %IMU系统输出的比力值和角速度

%%%%%%%%%通过GPS观测值计算得到的姿态角

zitaijiao_gps=xlsread('D:\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\');

%%%------------------------------------------------------------

%% 循环开始：for 1:n

outx=[];

outp=[];

detajiao=zeros(3,1);

NN =1000;

for i=1:NN

outx=[outx;x'];

outp=[outp;diag(P)'];

%大地子午圈曲率半径：RM

RM=a*(1-e2)/(1-e2*(sin(x(5)))^2)^(2/3);

%地球卯酉圈的曲率半径：RN

RN=a/sqrt(1-e2*(sin(x(5)))^2);