MATLAB,简单的导航仿真源码程序 - matlab通信信号

下载频道> 资源分类> matlab源码> 通信信号> MATLAB,简单的导航仿真源码程序

标题：MATLAB,简单的导航仿真源码程序
分享到：

所属分类: 通信信号资源类型：文件大小: 1.56 KB 上传时间: 2016-01-28 20:51:06 下载次数: 11 资源积分：1分提供者: xiaopeng2 点击下载

内容：
MATLAB,简单的导航仿真源码程序，程序员在编程的过程中可以参考学习使用，希望对IT程序员有用，此源码程序简单易懂、方便阅读，有很好的学习价值！

clc

clear

T=1;

times=1;

sum=zeros(100,1);

sum1=zeros(100,1);

sum2=zeros(100,1);

z=zeros(200,1);

for m=1:times

xri1=zeros(4,1);

xri2=zeros(4,1);

alpha=120*pi/180;%%%%target direction目标航向

alpha1=330*pi/180;%浮标航向

xri1=[-1000 0 0 0]';%浮标1状态向量

xri2=[0 0 0 0]';

xrt=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';%目标状态向量

for i=1:100

pha=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1]; %%%state transfer matrix状态转移矩阵

Oistate1=pha*xri1;%浮标1运动状态向量

Oistate2=pha*xri2;

Tistate=pha*xrt;%目标运动状态向量

%%%%%%%%%%%%%CRLBCRLBCRLB%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

TT(:,i)=Tistate;

O1(:,i)=Oistate1;

O2(:,i)=Oistate2;

r1(i,1)=(Tistate(1)-Oistate1(1))^2+(Tistate(2)-Oistate1(2))^2;

r2(i,1)=(Tistate(1)-Oistate2(1))^2+(Tistate(2)-Oistate2(2))^2;

for l=1:i

b1(l,:)=[(TT(2,l)-O1(2,l))/r1(l),-(TT(1,l)-O1(1,l))/r1(l),-(i-l)*(TT(2,l)-O1(2,l))/r1(l),(i-l)*(TT(1,l)-O1(1,l))/r1(l)];

b2(l,:)=[(TT(2,l)-O2(2,l))/r2(l),-(TT(1,l)-O2(1,l))/r2(l),-(i-l)*(TT(2,l)-O2(2,l))/r2(l),(i-l)*(TT(1,l)-O2(1,l))/r2(l)];

end

FIM=b1'*b1/(0.1*pi/180)^2+b2'*b2/(0.1*pi/180)^2;

FF=inv(FIM);

mse(i,:)=sqrt(FF(1,1)+FF(2,2));%%%%%%误差CRLB

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

V1=(0.1*pi/180)*randn;%噪声

V2=(0.1*pi/180)*randn;

beta1=atan((Tistate(1)-Oistate1(1))/(Tistate(2)-Oistate1(2)));

beta2=atan((Tistate(1)-Oistate2(1))/(Tistate(2)-Oistate2(2)));

betam1(i,:)=beta1+V1;%%%measure加噪声后的方位角测量值

betam2(i,:)=beta2+V2;%%%measure加噪声后的方位角测量值

H=[cos(betam1(i,:)) -sin(betam1(i,:)) 0 0;cos(betam2(i,:)) -sin(betam2(i,:)) 0 0];

Ak=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];%状态转移矩阵

z(2*i-1,:)=H(1,:)*Oistate1;% mathematic measure文献中的Z(t)

z(2*i,:)=H(2,:)*Oistate2;% mathematic measure

%A*iK=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];

A([2*i-1,2*i],:)=H*Ak;

theta(:,i)=inv(A([1:2*i],:)'*A([1:2*i],:))*A([1:2*i],:)'*z([1:2*i],:);

r11=sqrt((Tistate(1)-Oistate1(1,1))^2+(Tistate(2)-Oistate1(2,1))^2);

r22=sqrt((Tistate(1)-Oistate2(1,1))^2+(Tistate(2)-Oistate2(2,1))^2);

w(2*i-1,2*i-1)=r11^2*(0.1*pi/180)^2;

w(2*i,2*i)=r22^2*(0.1*pi/180)^2;

betam11(i,:)=beta1+V1;%atan((Tistate(1)+i*T*theta(3,1)-Oistate1(1))/(Tistate(2)+i*T*theta(4,1)-Oistate1(2)));

betam22(i,:)=beta2+V2;%atan((Tistate(1)+i*T*theta(3,1)-Oistate2(1))/(Tistate(2)+i*T*theta(4,1)-Oistate2(2)));

mmse(i,:)=sqrt((theta(1,i)-Tistate(1))^2+(theta(2,i)-Tistate(2))^2);%%%%%%误差

mmmse(i,:)=sqrt((theta(1,i)-Tistate(1))^2+(theta(2,i)-Tistate(2))^2);%%%%%%误差

A1([2*i-1,2*i],:)=H*Ak;

if i>1

u=0.01;%调节系数

theta(:,1)=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';

%if i<4

theta1(:,i)=theta(:,i-1)+inv(A([1:2*i-2],:)'*A([1:2*i-2],:)+u*eye(size(A([1:2*i-2],:)'*A([1:2*i-2],:))))*A([1:2*i-2],:)'*z([1:2*i-2],:);

theta1(:,1)=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';

% else

% theta1=TH+inv(A1([1:2*i-2],:)'*A1([1:2*i-2],:)+u*eye(size(A1([1:2*i-2],:)'*A1([1:2*i-2],:))))*A1([1:2*i-2],:)'*z([1:2*i-2],:);

% end

betam11(i,:)=atan((theta(1,1)+(i)*T*theta1(3,i)-Oistate1(1))/(theta(2,1)+(i)*T*theta1(4,i)-Oistate1(2)));

betam22(i,:)=atan((theta(1,1)+(i)*T*theta1(3,i)-Oistate2(1))/(theta(2,1)+(i)*T*theta1(4,i)-Oistate2(2)));

H1=[cos(betam11(i,:)) -sin(betam11(i,:)) 0 0;cos(betam22(i,:)) -sin(betam22(i,:)) 0 0];

A1([2*i-1,2*i],:)=H1*Ak;

r1t=sqrt((theta(1,1)+i*theta1(3,i)*T-Oistate1(1,1))^2+(theta(2,1)+i*theta1(4,i)*T-Oistate1(2,1))^2);

r2t=sqrt((theta(1,1)+i*theta1(3,i)*T-Oistate2(1,1))^2+(theta(2,1)+i*theta1(4,i)*T-Oistate2(2,1))^2);

w(2*i-1,2*i-1)=r1t^2*(0.1*pi/180)^2;

w(2*i,2*i)=r2t^2*(0.1*pi/180)^2;

TH=inv((inv(w([1:2*i],[1:2*i]))*A1([1:2*i],:))'*A([1:2*i],:))*(inv(w([1:2*i],[1:2*i]))*A1([1:2*i],:))'*z([1:2*i],:);

mmse(i,:)=sqrt((TH(1)-Tistate(1))^2+(TH(2)-Tistate(2))^2);%%%%%%误差

end

%theta=0;A=0;z=0;

sum=sum+mmse;

sum1=sum1+mse;

sum2=sum2+mmmse;

end

re=sum/times;

re1=sum1/times;

re11=sum2/times;

t=1:100;

plot(t,re,t,re1,'r',t,re11,'g');

文件列表（点击上边下载按钮，如果是垃圾文件请在下面评价差评或者投诉）:

MATLAB,简单的导航仿真源码程序/
MATLAB,简单的导航仿真源码程序/Untitled.m

关键词：源码 程序

matlab源码下载排行

回到顶部

友情链接

matlab源码网站源码编程语言网络编程手机通讯编程 KUNAG 官网 KUNAG 电商更多

: 联系方式| 版权声明| 招聘信息| 广告服务| 银行汇款| 法律顾问| 兼职技术| 付款方式| 关于我们|; 网站客服程序员兼职招聘 411241577@qq.com; 沪ICP备19040327号-3; 公安备案号：沪公网安备 31011802003874号; 库纳格流体控制系统（上海）有限公司版权所有; Copyright © 1999-2014, GUSUCODE.COM, All Rights Reserved

关键词： 源码 程序

相关推荐

matlab源码下载排行

友情链接

关键词：源码程序