加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
文件
该仓库未声明开源许可证文件(LICENSE),使用请关注具体项目描述及其代码上游依赖。
克隆/下载
LocalizaitonEKF.m 3.15 KB
一键复制 编辑 原始数据 按行查看 历史
gilbertz 提交于 2016-04-11 21:06 . 完成了粒子滤波
% function XE=LocationEKF(T,offtime,d)
%LocationEKF 采用卡尔曼滤波方法,从观测数值中得到航迹的最优估计
%XE 输出x轴方向上的误差
%T 采样时间,gps观测周期
%offtime 仿真时间
%d 噪声的标准差
clc;
clear;
T=1;
offtime=332;
d=0.1;
close all
N=ceil(offtime/T); %采样点数
A=zeros(7,7);
W=zeros(7,1);
X=zeros(7,1); % 一次的滤波输出值
C=zeros(4,7);
V=zeros(4,1);
Z=zeros(4,1);%一次的观测值
XE=zeros(7,N);%所有的预测值
Ve=zeros(N,1);
Vn=zeros(N,1);
ZE=zeros(4,N);
gx=zeros(N,1);
gy=zeros(N,1);
randn('state',sum(100*clock)); % 设置随机数发生器
%%%%%%%%%%%读取文本gps值、里程计位移和航向角,并转化为直角坐标系下的坐标值%%%%%%
fgps=fopen('VelocityDetector0325.txt','r');%%%打开文本
n=0;
while 1
gpsline=fgetl(fgps);%%%读取文本指针对应的行
if ~ischar(gpsline) break;%%%判断是否结束
end;
n=n+1;
time=sscanf(gpsline,'[Info] 2016-03-25%s(ViewController.m:%d)-[ViewController outputAccelertion:]:lat:%f;lon:%f;heading:%f;distance:%f');
data=sscanf(gpsline,'[Info] 2016-03-25 %*s (ViewController.m:%*d)-[ViewController outputAccelertion:]:lat:%f;lon:%f;heading:%f;distance:%f');
if(isempty(data))
break;
end
result=lonLat2Mercator(data(2,1),data(1,1));
gx(n)=result.X;%GPS经过坐标变换后的东向坐标,换算成米数
gy(n)=result.Y;%GPS经过坐标变换后的北向坐标,换算成米数
Phi(n)=data(3,1)*pi/180;%航向角
dd(n)=data(4,1);%某一周期的位移
dx(n)=dd(n)*sin(Phi(n))*4;%某一周期的东向位移
dy(n)=dd(n)*cos(Phi(n))*4;%某一周期的北向位移
Ve(n)=dd(n)*sin(Phi(n));%里程计输入的东向速度,暂时用某一周期的东向位移代替
Vn(n)=dd(n)*cos(Phi(n));%里程计输出的北向速度,暂时用某一周期的北向位移代替
ZE(:,n)=[gx(n),gy(n),dx(n),dy(n)];
end
fclose(fgps);%%%%%关闭文件指针
%过程向量A
A=[1,0,T,0,0,0,0;
0,1,0,T,0,0,0;
0,0,1,0,0,0,0;
0,0,0,1,0,0,0;
0,0,0,0,1,0,0;
0,0,0,0,0,1,0;
0,0,0,0,0,0,1;
];
%过程噪声协方差矩阵
Q=diag([0,0,d^2,d^2,d^2,d^2,d^2]);
Theta=CreateGauss(0,d,1,N);%GPS航迹和DR航迹的夹角
%观测噪声协方差矩阵
R=diag([d^2,d^2,d^2,d^2]);
Xfli=[gx(1),gy(1),0,0,0,0,0]'; %初始条件进行估计
Px=diag([0,0,d^2,d^2,d^2,d^2,d^2]); % 滤波输出误差均方差矩阵
for k=1:N
C=[1,0,0,0,1,0,0;
0,1,0,0,0,1,0;
0,0,cos(Theta(k)),-sin(Theta(k)),0,0,-Ve(k)*sin(Theta(k))-Vn(k)*cos(Theta(k));
0,0,sin(Theta(k)),cos(Theta(k)),0,0,Ve(k)*cos(Theta(k))-Vn(k)*sin(Theta(k));
];
XE(:,k)=Xfli;
Xest=A*Xfli; % 更新该时刻的预测值 ---kalman equation1
%Xes=A*Xef+Gamma*W(k-1); % 预测输出误差
Pxe=A*Px*A'+Q; % 预测误差的协方差阵 ---kalman equation2
K=Pxe*C'/(C*Pxe*C'+R); % Kalman滤波增益 ---kalman equation3
Z=ZE(:,k);
Xfli=Xest+K*(Z-C*Xest);% k时刻Kalman滤波器的输出值 ---kalman equation4
%Xef=(eye(2)-K*C)*Xes-K*vx(k);%滤波后输出的误差
Px=(eye(7)-K*C)*Pxe;%滤波输出误差均方差矩阵 ---kalman equation5
end
randTheta=rand(1,400)';
[x,y]=TrueRoute(T,offtime);
cordinatex=ZE(1,5);
cordinatey=ZE(2,5);
%显示滤波轨迹
figure
plot(x,y,'r');hold on;
plot(ZE(1,:),ZE(2,:),'g');hold on;
plot(XE(1,:),XE(2,:),'b');hold off;
axis([cordinatex-100 cordinatex+200 cordinatey-200 cordinatey+100]),grid on;
legend('真实轨迹','观测轨迹','目标滤波航迹');
axis equal;
马建仓 AI 助手
尝试更多
代码解读
代码找茬
代码优化