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

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

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

% 卡尔曼滤波器在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

00 0 -1/Tge 0 0 0 0 0

00 0 0 -1/Tgn 0 0 0 0

00 0 0 0 -1/Tgz 0 0 0

00 0 0 0 0 0 0 0

00 0 0 0 0 0 0 0

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

B=[00 0sigma_ge*sqrt(2/Tge) 0 0 0 0 0

00 00 sigma_gn*sqrt(2/Tgn) 0 0 0 0

00 00 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=[10 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('滤波后的观测量')

采用滤波的方法是抑制干扰的一种有效途径。

滤波的方式主要有硬件滤波和软件滤波两种，硬件滤波措施是使用较多的一

种方法，技术比较成熟，但同时也增加了设备，提高了成本，而且电子设备的增

加有可能带来新的干扰源采用软件滤波算法不需增加硬件设备，可靠性高，功

能多样，使用灵活，具有许多硬件滤波措施所不具备的优点，当然它需要占一定

的运行时间。本系统中采用软件滤波的方法来抑制噪声干扰。

软件滤波的方法主要有中值滤波法、算术平均滤波法、递推平均滤波法(又

称滑动平均滤波法)、防脉冲干扰平均滤波法(又称中位值平均滤波法)、一阶滞

后滤波法等.

---