哪位大神有GPS与捷联惯导组合导航的卡尔曼滤波算法的matlab仿真程序

在下面的仿真的代码中，理想的观测量不是真实数据，而是自生成的正弦波数据，在真实的应用场景中，应该是一系列的参考数据。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 卡尔曼滤波器在INS-GPS组合导航中应用仿真

% Author : lylogn

% Email : lylogn@gmailcom

% Company: BUAA-Dep3

% Time : 20130106

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 参考文献:

% [1] 邓正隆 惯导技术, 哈尔滨工业大学出版社2006

clear all;

%% 惯性-GPS组合导航模型参数初始化

we = 360/24/60/60pi/180; %地球自转角速度，弧度/s

psi = 10pi/180; %psi角度 / 弧度

Tge = 012;

Tgn = 010;

Tgz = 010; %这三个参数的含义详见参考文献

sigma_ge=1;

sigma_gn=1;

sigma_gz=1;

%% 连续空间系统状态方程

% X_dot(t) = A(t)X(t) + B(t)W(t)

A=[0 wesin(psi) -wecos(psi) 1 0 0 1 0 0;

-wesin(psi) 0 0 0 1 0 0 1 0;

wecos(psi) 0 0 0 0 1 0 0 1;

0 0 0 -1/Tge 0 0 0 0 0;

0 0 0 0 -1/Tgn 0 0 0 0;

0 0 0 0 0 -1/Tgz 0 0 0;

0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0;]; %状态转移矩阵

B=[0 0 0 sigma_gesqrt(2/Tge) 0 0 0 0 0;

0 0 0 0 sigma_gnsqrt(2/Tgn) 0 0 0 0;

0 0 0 0 0 sigma_gzsqrt(2/Tgz) 0 0 0;]';%输入控制矩阵

%% 转化为离散时间系统状态方程

% X(k+1) = FX(k) + GW(k)

T = 01;

[F,G]=c2d(A,B,T);

H=[1 0 0 0 0 0 0 0 0;

0 -sec(psi) 0 0 0 0 0 0 0;];%观测矩阵

%% 卡尔曼滤波器参数初始化

t=0:T:50-T;

length=size(t,2);

y=zeros(2,length);

Q=05^2eye(3); %系统噪声协方差

R=025^2eye(2); %测量噪声协方差

y(1,:)=2sin(pit05);

y(2,:)=2cos(pit05);

Z=y+sqrt(R)randn(2,length); %生成的含有噪声的假定观测值，2维

X=zeros(9,length); %状态估计值，9维

X(:,1)=[0,0,0,0,0,0,0,0,0]'; %状态估计初始值设定

P=eye(9); %状态估计协方差

%% 卡尔曼滤波算法迭代过程

for n=2:length

X(:,n)=FX(:,n-1);

P=FPF'+ GQG';

Kg=PH'/(HPH'+R);

X(:,n)=X(:,n)+Kg(Z(:,n)-HX(:,n));

P=(eye(9,9)-KgH)P;

end

%% 绘图代码

figure(1)

plot(y(1,:))

hold on;

plot(y(2,:))

hold off;

title('理想的观测量');

figure(2)

plot(Z(1,:))

hold on;

plot(Z(2,:))

hold off;

title('带有噪声的观测量');

figure(3)

plot(X(1,:))

hold on;

plot(X(2,:))

hold off;

title('滤波后的观测量');

航空和航天的天文导航都是在航海天文导航基础上发展起来的。航空天文导航跟踪的天体主要是亮度较强的恒星。航天中则要用到亮度较弱的恒星或其他天体。以天体作为参考点，可确定飞行器在空中的真航向。

常用的天文导航仪器有星体跟踪器、天文罗盘和六分仪等。星体跟踪器(星敏感器)能从天空背景中搜索、识别和跟踪星体，并测出跟踪器瞄准线相对于参考坐标系的角度；天文罗盘通过测量太阳或星体方向来指示飞行器的航向；六分仪通过对恒星或行星的测量而指示出飞行器的导航信息。最为典型的仪器就是六分仪，通过测量天体的高度信息来进行后续的导航解算。

天文导航利用画圆求交点的方法来确定观测点所在载体的位置信息，由此进行导航。具体过程如下：

（1）画圆：确定天体投影点所在位置信息为圆心，并确定该投影点到观测点在球面上的最短距离（即连接投影点和观测点的大圆上的弧度值）即为圆的半径，由此可以画出该位置圆；

如上图所示，投影点的位置信息由天体赤纬Dec和地方时角LHA共同决定，两者可以通过航海天文历得到；圆的半径即天体顶距z，由六分仪等测量仪器测得的天体高度角H计算得到。

（2）求交点

在上一步“画圆”的基础上，可以利用萨姆纳法或高度差法来求解两圆或三圆的交点（观测不同天体或不同时间观测同一天体可以得到不同的位置圆），即为观测点所在载体的位置信息。

姓名：魏子涵

学号：17020110045

转自：>

以上就是关于哪位大神有GPS与捷联惯导组合导航的卡尔曼滤波算法的matlab仿真程序全部的内容，包括:哪位大神有GPS与捷联惯导组合导航的卡尔曼滤波算法的matlab仿真程序、天文导航原理、浅谈惯性导航系统等相关内容解答，如果想了解更多相关内容，可以关注我们，你们的支持是我们更新的动力！