;dt];%机动加速度系数矩阵c=[10];%量测矩阵x=[0;0];%初始化状态矢量x_hat=[xxx];%初始化状态估计矩阵(三个模型状态矢量综合考虑)q=0.04;%过程随机噪声方差r=36;%量测随机噪声方差p_var=[b*q*b'b*q*b'b*q*b'];%初始化方差矩阵(三个模型状态矢量综合考虑)%数据保存数组初始化position=[];%真实位置pmeas=[];%位置量测值velocity=[];%真实速度u=0:1:2;%k=0:dt:tg%生成仿真模拟数据fori=0:dt:tgw=0.2*randn;%过程随机噪声,均值为0,方差为0.04v=6*randn;%量测随机噪声,均值为0,方差为36ifi<=30x=a*x+b*w+d*u(2);elseifi>30&i<100x=a*x+b*w+d*u(1);elseifi>100&i<150x=a*x+b*w+d*u(3);elseifi>150&i<180x=a*x+b*w+d*u(1);elsex=a*x+b*w+d*u(3);endy=c*x+v;%量测值计算position=[position;x(1)];%真实位置pmeas=[pmeas;y];%位置量测值velocity=[velocity;x(2)];%真实速度endsavemovedataabdcupositionvelocitypmeasdttgqrx_hatp_var3运行结果图一:机动方式:0-30秒:u=1;30-100秒:u=0;100-150秒:u=2;150-180秒:u0;180-200秒:u=2过程噪声方差:0.04量测噪声方差:36图一图二:机动方式:0-30秒:u=2;30-100秒:u=0;100-150秒:u=1;150-180秒:u=0;180-200秒:u=2过程噪声方差:4量测噪声方差:100图二
全文预览
