gps導航演算法用matlab模擬

1. 哪位大神有GPS與捷聯慣導組合導航的卡爾曼濾波演算法的matlab模擬程序

在下面的模擬的代碼中，理想的觀測量不是真實數據，而是自生成的正弦波數據，在真實的應用場景中，應該是一系列的參考數據。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡爾曼濾波器在INS-GPS組合導航中應用模擬
% Author : lylogn
% Email : [email protected]
% 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('濾波後的觀測量');

2. 利用matlab分別對三邊測量定位演算法和改進演算法進行模擬和驗證 急求源程序

%%清空環境變數

clc

clear

loaddata

%%數據累加作為網路輸入

[n,m]=size(X);

fori=1:n

y(i,1)=sum(X(1:i,1));

y(i,2)=sum(X(1:i,2));

y(i,3)=sum(X(1:i,3));

y(i,4)=sum(X(1:i,4));

y(i,5)=sum(X(1:i,5));

y(i,6)=sum(X(1:i,6));

end

%%網路參數初始化

a=0.3+rand(1)/4;

b1=0.3+rand(1)/4;

b2=0.3+rand(1)/4;

b3=0.3+rand(1)/4;

b4=0.3+rand(1)/4;

b5=0.3+rand(1)/4;

%%學習速率初始化

u1=0.0015;

u2=0.0015;

u3=0.0015;

u4=0.0015;

u5=0.0015;

%%權值閥值初始化

t=1;

w11=a;

w21=-y(1,1);

w22=2*b1/a;

w23=2*b2/a;

w24=2*b3/a;

w25=2*b4/a;

w26=2*b5/a;

w31=1+exp(-a*t);

w32=1+exp(-a*t);

w33=1+exp(-a*t);

w34=1+exp(-a*t);

w35=1+exp(-a*t);

w36=1+exp(-a*t);

theta=(1+exp(-a*t))*(b1*y(1,2)/a+b2*y(1,3)/a+b3*y(1,4)/a+b4*y(1,5)/a+b5*y(1,6)/a-y(1,1));

kk=1;

%%循環迭代

forj=1:10

%循環迭代

E(j)=0;

fori=1:30

%%網路輸出計算

t=i;

LB_b=1/(1+exp(-w11*t));%LB層輸出

LC_c1=LB_b*w21;%LC層輸出

LC_c2=y(i,2)*LB_b*w22;%LC層輸出

LC_c3=y(i,3)*LB_b*w23;%LC層輸出

LC_c4=y(i,4)*LB_b*w24;%LC層輸出

LC_c5=y(i,5)*LB_b*w25;%LC層輸出

LC_c6=y(i,6)*LB_b*w26;%LC層輸出

LD_d=w31*LC_c1+w32*LC_c2+w33*LC_c3+w34*LC_c4+w35*LC_c5+w36*LC_c6;%LD層輸出

theta=(1+exp(-w11*t))*(w22*y(i,2)/2+w23*y(i,3)/2+w24*y(i,4)/2+w25*y(i,5)/2+w26*y(i,6)/2-y(1,1));%閥值

ym=LD_d-theta;%網路輸出值

yc(i)=ym;

%%權值修正

error=ym-y(i,1);%計算誤差

E(j)=E(j)+abs(error);%誤差求和

error1=error*(1+exp(-w11*t));%計算誤差

error2=error*(1+exp(-w11*t));%計算誤差

error3=error*(1+exp(-w11*t));

error4=error*(1+exp(-w11*t));

error5=error*(1+exp(-w11*t));

error6=error*(1+exp(-w11*t));

error7=(1/(1+exp(-w11*t)))*(1-1/(1+exp(-w11*t)))*(w21*error1+w22*error2+w23*error3+w24*error4+w25*error5+w26*error6);

%修改權值

w22=w22-u1*error2*LB_b;

w23=w23-u2*error3*LB_b;

w24=w24-u3*error4*LB_b;

w25=w25-u4*error5*LB_b;

w26=w26-u5*error6*LB_b;

w11=w11+a*t*error7;

end

end

%畫誤差隨進化次數變化趨勢

figure(1)

plot(E)

title('訓練誤差','fontsize',12);

xlabel('進化次數','fontsize',12);

ylabel('誤差','fontsize',12);

%print-dtiff-r60028-3

%根據訓出的灰色神經網路進行預測

fori=31:36

t=i;

LB_b=1/(1+exp(-w11*t));%LB層輸出

LC_c1=LB_b*w21;%LC層輸出

LC_c2=y(i,2)*LB_b*w22;%LC層輸出

LC_c3=y(i,3)*LB_b*w23;%LC層輸出

LC_c4=y(i,4)*LB_b*w24;%LC層輸出

LC_c5=y(i,5)*LB_b*w25;

LC_c6=y(i,6)*LB_b*w26;

LD_d=w31*LC_c1+w32*LC_c2+w33*LC_c3+w34*LC_c4+w35*LC_c5+w36*LC_c6;%LD層輸出

theta=(1+exp(-w11*t))*(w22*y(i,2)/2+w23*y(i,3)/2+w24*y(i,4)/2+w25*y(i,5)/2+w26*y(i,6)/2-y(1,1));%閥值

ym=LD_d-theta;%網路輸出值

yc(i)=ym;

end

yc=yc*100000;

y(:,1)=y(:,1)*10000;

%計算預測的每月需求量

forj=36:-1:2

ys(j)=(yc(j)-yc(j-1))/10;

end

figure(2)

plot(ys(31:36),'-*');

holdon

plot(X(31:36,1)*10000,'r:o');