求两个matlab仿真程序，一个是捷联惯导系统的仿真 ，一个是组合导航系统的卡尔曼滤波算法仿真

求两个matlab仿真程序，一个是捷联惯导系统的仿真 ，一个是组合导航系统的卡尔曼滤波算法仿真

这。。。20分貌似

在下面的仿真的代码中，理想的观测量不是真实数据，而是自生成的正弦波数据，在真实的应用场景中，应该是一系列的参考数据。 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 卡尔曼滤波器在ins-gps组合导航中应用仿真 % author : lylogn % email : lylogn@gmail.com % company: buaa-dep3 % time : 2013.01.06 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 参考文献: % [1]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006. clear all; %% 惯性-gps组合导航模型参数初始化 we = 360/24/60/60*pi/180; %地球自转角速度，弧度/s psi = 10*pi/180; %psi角度 / 弧度 tge = 0.12; tgn = 0.10; tgz = 0.10; %这三个参数的含义详见参考文献 sigma_ge=1; sigma_gn=1; sigma_gz=1; %% 连续空间系统状态方程 % x_dot(t) = a(t)*x(t) + b(t)*w(t) a=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0; -we*sin(psi) 0 0 0 1 0 0 1 0; we*cos(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_ge*sqrt(2/tge) 0 0 0 0 0; 0 0 0 0 sigma_gn*sqrt(2/tgn) 0 0 0 0; 0 0 0 0 0 sigma_gz*sqrt(2/tgz) 0 0 0;]';%输入控制矩阵 %% 转化为离散时间系统状态方程 % x(k+1) = f*x(k) + g*w(k) t = 0.1; [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=0.5^2*eye(3); %系统噪声协方差 r=0.25^2*eye(2); %测量噪声协方差 y(1,:)=2*sin(pi*t*0.5); y(2,:)=2*cos(pi*t*0.5); 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)=f*x(:,n-1); p=f*p*f'+ g*q*g'; kg=p*h'/(h*p*h'+r); x(:,n)=x(:,n)+kg*(z(:,n)-h*x(:,n)); p=(eye(9,9)-kg*h)*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('滤波后的观测量');