基于RSSI的机器人室内卡尔曼滤波定位算法

1、精选优质文档-倾情为你奉上中北大学 课程设计说明书 2012-2013 学年第 一 学期学 院：信息与通信工程学院专 业：信息对抗技术专业学 生 姓 名：韩潇洒学 号：课程设计题目：信息处理实践基于RSSI的机器人室内卡尔曼滤波定位算法起 迄 日 期:8月20日 8月26日课程设计地点:校内指 导 教 师:杨光、王鉴、李凯、王玉、姚金杰系主任：赵冬娥下达任务书日期: 2012年8月 20 日中北大学课程设计任务书 2012/2013 学年第 一 学期  学 院： 信息与通信工程学院专 业： 信息对抗技术专业学 生 姓 名：韩潇洒学 号： 课程设

2、计题目：信息处理实践基于RSSI的机器人室内卡尔曼滤波定位算法起 迄 日 期:8月20日 8月26日课程设计地点: 校内指 导 教 师:杨光、王鉴、李凯、王玉、姚金杰系主任： 赵冬娥   下达任务书日期: 2012年 8 月 20 日专心-专注-专业课 程 设 计 任 务 书1设计目的：1、通过本课程设计的学习，学生将复习所学的专业知识，使课堂学习的理论知识应用于实践，通过本课程设计的实践使学生具有一定的实践操作能力；2、掌握Matlab使用方法，能熟练运用该软件设计并完成相应的信息处理；3、通过信息处理实践的课程设计，掌握设计信息处理系统的思

3、维方法和基本开发过程。2设计内容和要求（包括原始数据、技术参数、条件、设计要求等）：一、学习Matlab软件1、掌握Matlab软件的安装；2、熟悉Matlab的运行环境；3、根据所设计系统的需要会合理设定系统定时窗口的各项参数；4、根据所设计系统的需要，优化模块设计最优系统。二、实践设计要求：1、根据所选题目，设计实现系统的原理框图；2、建立系统的模型；3、每人一组，写出设计报告；4、根据给定位置信息，绘制二维路线图；5、设计卡尔曼滤波算法；6、仿真卡尔曼滤波定位算法。三、参考题目基于RSSI的机器人室内卡尔曼滤波定位算法课 程 设 计 任 务 书3设计工作任务及工作量的要求包括课程设计计算

4、说明书(论文)、图纸、实物样品等：每个同学独立完成自己的任务，每人写一份设计报告，在课程设计论文中写明自己设计的部分，给出设计结果。4主要参考文献：1. 樊昌信等.通信原理（第三版）.北京：国防工业出版社.20042. 郑君里等.信号与系统（第二版）.北京：高等教育出版社.20003. 安 雷、张国良、汤文俊.基于RSSI的机器人室内卡尔曼滤波定位算法研究5设计成果形式及要求： 设计的原理框图，仿真模型。 课程设计说明书  6工作计划及进度：8月20日 8月21日8月21日 8月22日8月22 日 8月24日8月24日 8月26日8月26日相关资料查阅；相关软件的

5、学习；方案设计，完成并实现信息处理；书写论文；答辩、成绩考核。系主任审查意见：  签字： 年 月 日目 录一、 设计目的 4二、 设计任务与要求及软件介绍 4三、 设计步骤 51、 实验预习 52、 系统原理图以及系统模型 63、 设计结果 10四、 设计总结 16五、 实验心得 18六、 参考文献 18七、 附录（源代码）181、 设计目的1、通过本课程设计的学习，学生将复习所学的专业知识，使课堂学习的理论知识应用于实践，通过本课程设计的实践使学生具有一定的实践操作能力；2、掌握Matlab使用方法，能熟练运用该软件设计并完成相应的信息处理；3、通过信息处理实践的课程设计，掌握设计

6、信息处理系统的思维方法和基本开发过程。4、在上学期的基础上，使定位节点（机器人）在室内做匀速运动和匀加速运动，实现拉尔曼滤波定位算法。2、 设计任务与要求及软件介绍任务： 用RSSI方法实现机器人室内定位，继而进行卡尔曼滤波定位算法。要求：首先根据所选题目，设计实现系统的原理框图，并建立系统的模型：装用八个测量节点的实验室（长17m，宽7m）。其次用Matlab仿真二维路线图（），产生模拟距离数据,进而用定位算法得到机器人位置,以及根据给定位置信息，绘制二维路线图。再次设计卡尔曼滤波算法，用Matlab仿真卡尔曼滤波定位算法，得到机器人位置。最后写出设计报告。软件介绍：MATLAB是由美国ma

7、thworks公司发布的主要面对科学计算、可视化以及交互式程序设计的高科技计算环境。它将数值分析、科学数据可视化以及非动态系统的和仿真等诸多强大功能集成在一个易于使用的视窗环境中，为科学研究、工程设计以及必须进行有效数值计算的众多科学提供了一种全面的解决方案，并在很大程度上摆脱了传统非交互式程序设计语言（如C、Fortran）的编辑模式，代表了当今国际科学计算软件的先进水平。MATLAB和、并称为三大软件。它在数学类科技应用软件中在方面首屈一指。MATLAB可以进行矩阵运算、绘制和数据、实现、创建用户界面、连 matlab开发工作界面接其他编程语言的程序等，主要应用于工程计算、控制设计、信号处

8、理与通讯、设计与分析等领域。 MATLAB的基本数据单位是，它的指令表达式与数学、工程中常用的形式十分相似，故用MATLAB来解算问题要比用C，FORTRAN等语言完成相同的事情简捷得多，并且MATLAB也吸收了像Maple等软件的优点，使MATLAB成为一个强大的。在新的版本中也加入了对，的支持。可以直接调用,用户也可以将自己编写的实用程序导入到MATLAB函数库中方便自己以后调用，此外许多的MATLAB爱好者都编写了一些经典的程序，用户可以直接进行下载就可以用。三、设计步骤1、实验预习 根据此次课程设计要求：首先，根据所选题目，设计实现系统的原理框图，并建立系统的模型。其次，查阅有关Mat

9、lab的相关书籍、文章以及网络资源。熟悉Matlab软件的使用。其次，根据系统原理图和系统模型，用Matlab仿真产生机器人二维路线图并获得仿真机器人位置数据，继而与节点结合产生距离数据，在用定位算法求得机器人位置数据，并加入随机序列白噪声。最后，设计卡尔曼滤波算法。并用Matlab仿真卡尔曼滤波定位算法，对加有噪声进行滤波。写出设计报告；3、系统原理框图以及系统模型根据系统节点和距离信息，用定位模型求解位置信息根据系统节点位置，获得距离信息Matlab 仿真产生二维路径，即二维位置信息随机序列噪声通过卡尔曼滤波器，对位置信息处理获得加有噪声的位置信息图1. 系统原理框图 该定位系统是由8个参

10、考节点和1个定位节点组成的Zig-Bee 无线传感器网络。如图2，室内节点布置俯视图，18为8个固定参考节点，还有一个定位节点。参考节点地址以及坐标见表一。该定位系统的工作过程，先通过定位节点采集8个参考节点的信号强度，根据信号强度以及相关无线电理论求得定位节点与8个参考节点之间的距离。然后利用距离信息和参考节点的位置建立定位模型，从而确定定位节点的位置。再者建立卡尔曼滤波模型，对定位节点的位置信息滤波处理。表一参考节点设置表参考节点地址坐标/参考节点地址坐标/0X0001(0,0,2)0X0101(16,6,2)0X0010(0,3.5,2)0X0110(16,3.5,2)0X0011(0,

11、6,2)0X0111(16,0,2)0X0100(8,6,2)0X1000(8,0,2)室内定位模型建立本文利用各个参考节点采集定位节点的RSSI值，依据RSSI值和距离的特定关系，建立RSSI值和距离的数学模型为： （1）式中，n 为信号传播常量；d 为距发射器的距离；A为距离1米时的RSSI值。假设参考节点的空间位置为，定位节点的空间位置为，定位节点和参考节点之间的距离为，得到方程： （2）为求解方程，至少需要4个参考节点，对公式（2）进行线性化处理。将式（2）在近似位置处展开成线性方程，写成矩阵形式为： （3）式中，；。令，则式（3）可写成下列形式：，即 （4）当参考节点多于4个时，方程

12、（4）为矛盾方程。另一个计算方法为： （5）上述等式的求解可以采用最小二乘法（Least SquaresMethod，LSM）来进行迭代计算，即先从定位节点位置和传输距离误差的大概值开始，然后逐步精确到计算结果满足测量要求并将此值作为定位的最终结果。该方法求解的优点是在利用计算机求解时，可以尽可能多地利用各种有价值的信息，减小求解过程中引入的误差。卡尔曼滤波模型的建立基于RSSI定位的重点是距离的测量，而RSSI测距容易受到干扰噪声的影响，上述方法已不能满足机器人对定位精度的要求。卡尔曼滤波是高斯过程最优滤波的一种有效算法，当对象模型足够准确且系统状态和参数不发生突变时，性能较好。因此，将卡尔

13、曼滤波与上述方法相结合，利用卡尔曼滤波器的滤波功能减小噪声对系统的影响，达到提高定位精度的目的。首先是根据上述的定位信息建立系统的位移和速度的状态方程并进行离散化。定位系统状态方程为： （6） （7）式中，状态向量为待优化的机器人定位信息，和分别为 k 时刻机器人在坐标系中三个方向的位移和速度估计值；A为系统矩阵；观测向量S(k)为观测的机器人定位信息，为 k 时刻机器人在坐标系中三个方向的位移的观测值；C 为输出矩阵；和分别为状态噪声和观测噪声，且满足，即W(k)和V(k)是相互独立的零均值白噪声序列。状态向量的初始值 X(0) 的统计特性给定为：卡尔曼滤波器计算分为：（1）卡尔曼滤波器方程

14、的预测过程： （8） （9）（2）卡尔曼滤波器方程的校正过程： （10） （11） （12）用户位置移动的三维模型为： （13） （14）初值的选取：，5、设计结果匀速运动：图3. 仿真机器人匀速运动二维路线图 图4. 机器人匀速运动x方向位置 图5.机器人匀速运动y方向位置Matlab仿真产生的机器人室内二维路线图，如图3。相应方向、方向的位置。图6.机器人匀速运动真实、观测位置信息图7.机器人匀速运动x、y观测位置信息图8.机器人匀速运动x方向真实、观测、滤波位置信息图9.机器人匀速运动y方向真实、观测、滤波位置信息 Matlab仿真的机器人真实、观测位置信息，如图6。仿真产生的x、y方向

15、的观测位置信息，如图7。用Matlab仿真卡尔曼滤波算法，并用其对观测位置信息滤波，如图8、图9。匀加速运动：图10. 仿真机器人匀加速运动二维路线图 图11. 机器人匀加速运动x方向位置 图12.机器人匀加速运动y方向位置图13.机器人匀加速运动真实、观测位置信息图14.机器人匀加速运动x、y观测位置信息图15.机器人匀加速运动x方向真实、观测、滤波位置信息图16.机器人匀加速运动y方向真实、观测、滤波位置信息Matlab仿真的匀加速运动的机器人真实、观测位置信息，如图13。仿真产生的x、y方向的观测位置信息，如图14。用Matlab仿真卡尔曼滤波算法，并用其对观测位置信息滤波，如图15、图

16、16。 四、设计总结根据前面4-5页的软件介绍，我知道Matlab是一款功能齐全的数据处理软件。首先，用Matlab仿真产生机器人二维位置信息（如图3、10），为后续加噪声、滤波等信息处理做准备。产生方法如下：% 产生模拟理论位置数据% 匀速运动t = 0:0.1:12.9; v = 1; s0 = 1;s = s0 + v*t;x = s;y = 3*x./x;% 匀加速运动t = 0:0.1:5; s0 = 0.1; v0 = 0; a = 1;s = s0 + v0*t +1/2.*a*t.2;x = sqrt(3)/2.*s;y = 1/2.*s;其次，根据节点信息将二维位置信息转为距

17、离信息，再用距离信息求机器人位置信息，然后对机器人位置信息加噪声（如图6、7和图13、14）。% 转位置数据为距离数据Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:nfor i = 1:8 d(m,i) = sqrt(x(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 +(z(m)-Rs(i,3)2);endend最后，用Matlab仿真卡尔曼滤波算法，并用其对加有噪声机器人位置信息滤波（如图8、9和图15、16）。卡尔曼滤波算法如下

18、：% 卡尔曼滤波 N = n; w = randn(1,N); %系统预测的随机白噪声w(1) = 0;xs1(1) = 0; a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %系统的预测值end V = randn(1,N); %测量值的随机白噪声q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.1;Y1 = c.*xd + c1.*V; %测量值p(1) = 0; p2(1) = 0; s1(1) = 0;s2

19、(1) = 0;for t = 2:N; p1(t) = a.2*p(t-1) + Rww; %前一时刻X的相关系数b(t) = c*p1(t)/(c.2*p1(t) + Rvv); %卡尔曼增益s1(t) = a*s1(t-1) + b1(t)*(Y1(t)-a*c*s1(t-1); %经过滤波后的信号p(t) = p1(t) - c*b(t)*p1(t);%t状态下x(t|t)的相关系数end五、 实验心得6、 参考文献 1.樊昌信等.通信原理（第三版）.北京：国防工业出版社.20042.郑君里等.信号与系统（第二版）.北京：高等教育出版社.20003.安 雷、张国良、汤文俊.基于RSSI

20、的机器人室内卡尔曼滤波定位算法研究4.彭 渤.基于RSSI测距误差补偿的无线传感器网络定位算法研究5.朱登科. 基于 RSSI 的无线传感器网络测距和定位技术研究七、附录（源代码）% 产生模拟理论位置数据close all;clear all;clc;% 匀加速运动t = 0:0.1:5;s0 = 0.1;v0 = 0;a = 1;s = s0 + v0*t +1/2.*a*t.2;x = sqrt(3)/2.*s;y = 1/2.*s;z = 0.*x;figure;plot(x,y,'r.-');grid on;legend('真实位置');title(&#

21、39;二维线路图');xlabel('长：x/m');ylabel('宽：y/m');figure;plot(x,'r.-');xlabel('采样点');ylabel('x方向位置/m');title('x方向线路图');grid on;legend('x方向真实位置');figure;plot(y,'k.-');xlabel('采样点');ylabel('y方向位置/m');title('y方向线路图');g

22、rid on;legend('y方向真实位置');% 转位置数据为距离数据Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:n for i = 1:8 d(m,i) = sqrt(x(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 + (z(m)-Rs(i,3)2); endend % 保存数据为.matxs = x;save('xs.mat','xs');ys = y;save('

23、ys.mat','ys');zs = z;save('zs.mat','zs');ds = d;save('ds.mat','ds');% 用模拟值定位close all;clear all;clc;Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;D = load('ds.mat');X = load('xs.mat');Y = load('ys.mat');n = length(D.ds

24、);x = zeros(n,3);zd = 0;for m = 1:n for i=1:8 d(m,i) = D.ds(m,i); endendfor m = 1:n xs(m) = X.xs(m); ys(m) = Y.ys(m);endfor m = 1:n for xd = 0:0.001:17 for yd = 0:0.001:7 if (abs(xd-Rs(1,1)2+(yd-Rs(1,2)2+(zd-Rs(1,3)2-d(m,1)2)<0.1&&. abs(xd-Rs(2,1)2+(yd-Rs(2,2)2+(zd-Rs(2,3)2-d(m,2)2)<0.

25、1&&. abs(xd-Rs(3,1)2+(yd-Rs(3,2)2+(zd-Rs(3,3)2-d(m,3)2)<0.1&&. abs(xd-Rs(4,1)2+(yd-Rs(4,2)2+(zd-Rs(4,3)2-d(m,4)2)<0.1&&. abs(xd-Rs(5,1)2+(yd-Rs(5,2)2+(zd-Rs(5,3)2-d(m,5)2)<0.1&&. abs(xd-Rs(6,1)2+(yd-Rs(6,2)2+(zd-Rs(6,3)2-d(m,6)2)<0.1&&. abs(xd-Rs(7

26、,1)2+(yd-Rs(7,2)2+(zd-Rs(7,3)2-d(m,7)2)<0.1&&. abs(xd-Rs(8,1)2+(yd-Rs(8,2)2+(zd-Rs(8,3)2-d(m,8)2)<0.1) x(m,1) = xd; x(m,2) = yd; x(m,3) = zd; else end end endendfor m = 1:n xd(m) = x(m,1); yd(m) = x(m,2);endxd = xd + 0.2.*randn(1,n);yd = yd + 0.2.*randn(1,n);figure;plot(xs,ys,'r.-&

27、#39;);hold on;plot(xd,yd,'o-');grid on;legend('真实位置','观测位置');title('二维线路图');xlabel('长：x/m');ylabel('宽：y/m');hold off;save('xd.mat','xd');save('yd.mat','yd');% 卡尔曼滤波 Xs = load('xs.mat');Ys = load('ys.mat')

28、;Xd = load('xd.mat');Yd = load('yd.mat');n = length(Xs.xs);for i1 = 1:n xs(i1) = Xs.xs(i1); ys(i1) = Ys.ys(i1); xd(i1) = Xd.xd(i1); yd(i1) = Yd.yd(i1);endN = n; w = randn(1,N); %系统预测的随机白噪声w(1) = 0;xs1(1) = 0; ys1(1) = 0;a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %x方向系统的预测值ys1(

29、k) = a*ys1(k-1) + w(k-1); %y方向系统的预测值end V = randn(1,N); %测量值的随机白噪声q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q22 = std(ys1);Ryy = q22.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.05;Y1 = c.*xd + c1.*V; %x方向测量值Y2 = c.*yd + c1.*V; %y方向测量值p(1) = 20; p2(1) = 20; s1(1) = 0;s2(1) = 0;for t = 2:N; p1(t

30、) = a.2*p(t-1) + Rww; %前一时刻X的相关系数p3(t) = a.2*p2(t-1) + Rww; %前一时刻Y的相关系数b(t) = c*p1(t)/(c.2*p1(t) + Rvv); %x方向卡尔曼增益b1(t) = c*p1(t)/(c.2*p1(t) + Rvv);%y方向卡尔曼增益s1(t) = a*s1(t-1) + b1(t)*(Y1(t)-a*c*s1(t-1); %x方向经过滤波后的信号s2(t)= a*s2(t-1) + b(t)*(Y2(t)-a*c*s2(t-1); %y方向经过滤波后的信号p(t) = p1(t) - c*b(t)*p1(t);%

31、t状态下x(t|t)的相关系数p2(t) = p3(t) - c*b(t)*p3(t);%t状态下y(t|t)的相关系数end figure(2);plot(xs1,'r.-');grid on;hold on;plot(ys1,'b-');hold off;title('系统的预测值');figure(3);plot(Y1,'k.-');grid on;hold on;plot(Y2,'r.-');hold off;title('测量值');legend('x方向观测值','

32、;y方向观测值');figure(4);plot(s1,'k.-');grid on;hold on;plot(xs,'r*-');hold off;hold on;plot(xd,'bo-');hold off;title('x方向定位效果');legend('x方向滤波值','x方向真实值','x方向观测值');xlabel('采样点');ylabel('x方向位置/m');figure(5);plot(s2,'r.-');

33、grid on;hold on;plot(ys,'b*-');hold off;hold on;plot(yd,'ko-');hold off;title('y方向定位效果');legend('y方向滤波值','y方向真实值','y方向观测值');xlabel('采样点');ylabel('y方向位置/m');% 真实值、观测值、滤波值的均值和方差% x方向xs_mean = mean(xs);xs_var = var(xs);xd_mean = mean(xd);x

34、d_var = var(xd);s1_mean = mean(s1);s1_var = var(s1);% y方向ys_mean = mean(ys);ys_var = var(ys);yd_mean = mean(yd);yd_var = var(yd);s2_mean = mean(s2);s2_var = var(s2);% 保存数据为.matsave('s1.mat','s1');save('s2.mat','s2');% 产生模拟理论位置数据close all;clear all;clc;% 匀速运动t = 0:0.1:

35、12.9;v = 1;s0 = 1;s = s0 + v*t;x = s;y = 3*x./x;z = 0.*x;figure;plot(x,y,'r.-');grid on;legend('真实位置');title('二维线路图');xlabel('长：x/m');ylabel('宽：y/m');figure;plot(x,'r.-');xlabel('采样点');ylabel('x方向位置/m');title('x方向线路图');grid on;l

36、egend('x方向真实位置');figure;plot(y,'k.-');xlabel('采样点');ylabel('y方向位置/m');title('y方向线路图');grid on;legend('y方向真实位置');% 转位置数据为距离数据Rs = 0,0,2;0,3.5,2;0,6,2;8,6,2;16,6,2;16,3.5,2;16,0,2;8,0,2;n = length(x);d = zeros(n,8);for m = 1:n for i = 1:8 d(m,i) = sqrt(x

37、(m)-Rs(i,1)2 + (y(m)-Rs(i,2)2 + (z(m)-Rs(i,3)2); endend % 保存数据为.matxs = x;save('xs.mat','xs');ys = y;save('ys.mat','ys');zs = z;save('zs.mat','zs');ds = d;save('ds.mat','ds');% 用模拟值定位close all;clear all;clc;Rs = 0,0,2;0,3.5,2;0,6,2;8,6,

38、2;16,6,2;16,3.5,2;16,0,2;8,0,2;D = load('ds.mat');X = load('xs.mat');Y = load('ys.mat');n = length(D.ds);x = zeros(n,3);zd = 0;for m = 1:n for i=1:8 d(m,i) = D.ds(m,i); endendfor m = 1:n xs(m) = X.xs(m); ys(m) = Y.ys(m);endfor m = 1:n for xd = 0:0.01:17 for yd = 0:0.01:7 if (

39、abs(xd-Rs(1,1)2+(yd-Rs(1,2)2+(zd-Rs(1,3)2-d(m,1)2)<0.1&&. abs(xd-Rs(2,1)2+(yd-Rs(2,2)2+(zd-Rs(2,3)2-d(m,2)2)<0.1&&. abs(xd-Rs(3,1)2+(yd-Rs(3,2)2+(zd-Rs(3,3)2-d(m,3)2)<0.1&&. abs(xd-Rs(4,1)2+(yd-Rs(4,2)2+(zd-Rs(4,3)2-d(m,4)2)<0.1&&. abs(xd-Rs(5,1)2+(yd-Rs(5

40、,2)2+(zd-Rs(5,3)2-d(m,5)2)<0.1&&. abs(xd-Rs(6,1)2+(yd-Rs(6,2)2+(zd-Rs(6,3)2-d(m,6)2)<0.1&&. abs(xd-Rs(7,1)2+(yd-Rs(7,2)2+(zd-Rs(7,3)2-d(m,7)2)<0.1&&. abs(xd-Rs(8,1)2+(yd-Rs(8,2)2+(zd-Rs(8,3)2-d(m,8)2)<0.1) x(m,1) = xd; x(m,2) = yd; x(m,3) = zd; else end end endend

41、for m = 1:n xd(m) = x(m,1); yd(m) = x(m,2);endxd = xd + 0.2.*randn(1,n);yd = yd + 0.2.*randn(1,n);figure;plot(xs,ys,'r.-');hold on;plot(xd,yd,'o-');grid on;legend('真实位置','观测位置');title('二维线路图');xlabel('长：x/m');ylabel('宽：y/m');hold off;save('

42、xd.mat','xd');save('yd.mat','yd');% 卡尔曼滤波 Xs = load('xs.mat');Ys = load('ys.mat');Xd = load('xd.mat');Yd = load('yd.mat');n = length(Xs.xs);for i1 = 1:n xs(i1) = Xs.xs(i1); ys(i1) = Ys.ys(i1); xd(i1) = Xd.xd(i1); yd(i1) = Yd.yd(i1);endN = n

43、; w = randn(1,N); %系统预测的随机白噪声w(1) = 0;xs1(1) = 0; ys1(1) = 0;a = 1; for k = 2:N; xs1(k) = a*xs1(k-1) + w(k-1); %x方向系统的预测值ys1(k) = a*ys1(k-1) + w(k-1); %y方向系统的预测值end V = randn(1,N); %测量值的随机白噪声q1 = std(V); Rvv = q1.2; q2 = std(xs1); Rxx = q2.2;q22 = std(ys1);Ryy = q22.2;q3 = std(w); Rww = q3.2; c = 1; c1 = 0.05;Y1 = c.*xd + c1.*V; %x方向测量值Y2 = c.*yd + c1.*V; %y方向测量值p(1) = 20; p2(1) = 20; s1(1) = 0;s2(1) = 0;for t = 2:N; p1(t)