1.matlab中数组的index放在()中,而不是[]中
2.sscanf的用法明显念销盯不是matlab的:不符合sscanf格式的定义
3.&string[11]实现的是指针的功能,这在c或c++语言中有斗氏,而matlab中没有。
不过sscanf倒是容易理解:在string中,从string[11]开始存储‘秒’的信息,用&string[11]将‘秒’信息的起始地址传给sscanf,sscanf再按照%1f的格式读取该信息,并将信息存到dsecond中,由于是使用指针进行 *** 作,所以不需要指定‘秒’信息的长度
在下面的仿真的代码中,理想的观测量不是真实数据,而是自生成的正弦波数据,在真实的应用场景中,应该是一系列的参考数键凳没据。%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波器在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('滤波后的观测量')
给你一个matlab程序,自己运行一下就知道了。clearclc
v0=90
vw=450
xw=0
yw=0
xr=0
yr=120
dt=0.001
t=0
figure('units','normalized')
axis([0 yr,0 yr+10])
set(gcf,'color','w')
w=line(xw,yw,'color','r','marker','.','markersize',20,'erasemode','xor')
r=line(xr,yr,'color','b','marker','.','markersize',10,'erasemode','xor')
wd=line(xw,yw,'color','r','linestyle','-','linewidth',2,'erasemode','none'迹如)
rd=line(xr,yr,'color','c','linestyle','-','linewidth',2,'erasemode'姿裂启,'none')
while (abs(1-xw/xr)>=1e-3||abs(1-yw/yr)>=1e-3)
xr=v0*t
l=sqrt((xw-xr)^2+(yw-yr)^2)
vx=vw*(xr-xw)/l
vy=vw*(yr-yw)/l
xw=xw+vx*dt
yw=yw+vy*dt
set(w,'xdata',xw,'ydata',yw)
set(wd,'xdata',xw,'源祥ydata',yw)
set(r,'xdata',xr,'ydata',yr)
set(rd,'xdata',xr,'ydata',yr)
drawnow
t=t+dt
end
hold on
fill([xw-5 xw xw+5 xw+2.5 xw+5 xw xw-5 xw-2.5],[yw-5 yw-2.5 yw-5 yw yw+5 yw+2.5 yw+5 yw],'r')
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)