CV、CA、CT运动模型的理解和matlab程序简单实现.pdf

1、CV、CA、CT运动模型的理解和matlab程序简单实现较为详细介绍的是CT，CV和CA可以类着快速理解、CV模型简单介绍%CV模型%xk=xk-1+vxk*delta_T+0.5*ax*delta_T2%vxk=vxk-1+ax*delta_T%yk=yk-1+vyk*delta_T+0.5*ay*delta_T2%vyk=vyk-1+ay*delta_T%X=x;vx;y;vy;cv_M=20;cv_X=zeros(4,cv_M);cv_delta_t=1;cv_noise_sigma=2 0;0 1.5;%加速度噪声 可调节，有不同的变化cv_noise_sigma2=cv_noise_

2、sigma*cv_noise_sigma;%差 cv_a_noise=cv_noise_sigma2*randn(2,1);cv_F=1,cv_delta_t,0,0;0,1,0,0;0,0,1,cv_delta_t;0,0,0,1;cv_G=0.5*cv_delta_t2,0;cv_delta_t,0;0,0.5*cv_delta_t2;0,cv_delta_t;cv_x0=50;2;70;3;subplot(1,3,1);for i=1:cv_M cv_X(:,i)=cv_F*cv_x0+cv_G*cv_a_noise;cv_x0=cv_X(:,i);plot(cv_X(1,i),cv_X

3、(3,i),g.);text(cv_X(1,i),cv_X(3,i),num2str(i);hold onendtitle(CV模型);、CA模型简单介绍%CA模型%会涉及到变加速直线运动的公式%a=a0+r*t%v=v0+a0*t+0.5*r*t2%s=v0*t+0.5*a0*t2+(1/6)*r*t3%状态值X=x;vx;ax;y;vy;ay,6个值%xk+1 =xk+vxk*t+0.5*axk*t2+(1/6)*rx*t3 rx符合正态分布%vxk+1=vxk+axk*t+0.5*rx*t2%axk+1=axk+rx*t%yk+1 =yk+vyk*t+0.5*ayk*t2+(1/6)*r

4、y*t3 ry符合正态分布%vyk+1=vyk+ayk*t+0.5*ry*t2%ayk+1=ayk+ry*t ca_delta_t=1;ca_M=20;ca_X=zeros(6,ca_M);ca_x0=60;8;1;55;3;2;%初值ca_noise_sigma=0.2 0;0 0.3;%噪声的标准差ca_noise_sigma2=ca_noise_sigma*ca_noise_sigma;%噪声的差ca_a_noise=ca_noise_sigma2*randn(2,1);ca_F=1,ca_delta_t,0.5*ca_delta_t2,0,0,0;0,1,ca_delta_t,0,0,

5、0;0,0,1,0,0,0;0,0,0,1,ca_delta_t,0.5*ca_delta_t2;0,0,0,0,1,ca_delta_t;0,0,0,0,0,1;%系数矩阵ca_G=(1/6)*ca_delta_t3,0;0.5*ca_delta_t2,0;ca_delta_t,0;0,(1/6)*ca_delta_t3;0,0.5*ca_delta_t2;0,ca_delta_t;subplot(1,3,2);for i=1:ca_M ca_X(:,i)=ca_F*ca_x0+ca_G*ca_a_noise;ca_x0=ca_X(:,i);plot(ca_X(1,i),ca_X(4,i),

6、b.);text(ca_X(1,i),ca_X(4,i),num2str(i);hold onendtitle(CA模型);三、CT模型简单介绍%CT模型%产个随机分布的指定均值和差的矩阵：将randn产的结果乘以标准差，然后加上期望均值即可。%例如，产均值为0.6，差为0.1的个5*5的随机数式如下：x=0.6+sqrt(0.1)*randn(5)delta_t=1;noise_sigma=1 0 0;%x向加速度的标准差值 0 1 0;%y向加速度的标准差值 0 0 0.05;%速度的标准差值。标准差，只涉及到三个噪声五个？%如果是5个噪声的话，分别是：x的加速度噪声，vx的加速度噪声，y

7、的加速度噪声，vy的加速度的噪声，omega的噪声%如果是3个噪声的话，表明x和vx的噪声是同个噪声，y和vy的噪声是同个，omega的噪声是个%因此，如果采的是5个噪声，那么噪声系数必然是5列的；但是这采了3个噪声，因此噪声系数就是3列了%我们的状态值选取的是x;vx;y;vy;omega是5的，因此噪声系数是 53列的，与31列的噪声V相乘后得到51列%CT运动模型的公式或者说程：恒定速度和恒定速度。略去公式推导了%xk+1 =xk+(sinwt)/w*vxk-(1-coswt)/w*vyk+0.5*ax*t2 ax符合N(0,noise_sigma_x)%vxk+1=(coswt)*vx

8、k-(sinwt)*vyk+ax*t%yk+1 =(1-coswt)/w*vxk+yk+(sinwt)/w*vyk+0.5*ay*t2 ay符合N(0,noise_sigma_y)%vyk+1=(sinwt)*vxk+(coswt)*vyk+ay*t%wk=wk+1+aw*t%恒定速度%写成矩阵形式就可以得到 运动系数F 和 噪声系数G%Xk+1=F*Xk+G*a_noiseG=0.5*delta_t2,0,0;delta_t,0,0;0,0.5*delta_t2,0;0,delta_t,0;0,0,delta_t;%为常系数，F并不是常系数（如果w有变化的话）！？noise_sigma2=n

9、oise_sigma.*noise_sigma;%差值a_noise=noise_sigma2*randn(3,1);%结果是得到31列的矩阵值,代表加速度值%运动程必须初始化初值即最开始的速度、位置、速度值需要给定或者通过其他式要获得，有初值才能递推M=20;%跟踪时长X=zeros(5,M);%标状态值 所有时刻下的x0=100;10;100;5;0.02;%初始坐标时(100,100),初始速度是(10,5),初始速度是0.02 这是单个标的%x=x0(1,1),vx=x0(2,1),y=x0(3,1),vy=x0(4,1),w=x0(5,1)%x0=100 20;0 0;100 50;

10、0 0;0.01 0.02;%这是两个标的即跟踪两个标subplot(1,3,3);%单个标下的CT运动模型for i=1:M%遍历每个时刻即计算每个时刻的状态值 sinwt=sin(x0(5,1)*delta_t);coswt=cos(x0(5,1)*delta_t);X(1,i)=x0(1,1)+sinwt*x0(2,1)/x0(5,1)-(1-coswt)*x0(4,1)/x0(5,1);X(2,i)=(coswt)*x0(2,1)-sinwt*x0(4,1);X(3,i)=(1-coswt)*x0(2,1)/x0(5,1)+x0(3,1)+sinwt*x0(4,1)/x0(5,1);X

11、(4,i)=sinwt*x0(2,1)+(coswt)*x0(4,1);X(5,i)=x0(5,1);%这是还未加噪声时的%x0=X(:,i);%实现迭代%plot(X(1,i),X(3,i),k.);pause(0.5);hold on%加噪声 a_noise=noise_sigma2*randn(3,1);X(:,i)=X(:,i)+G*a_noise;x0=X(:,i);%实现迭代 plot(X(1,i),X(3,i),k.);text(X(1,i),X(3,i),num2str(i);hold onend title(CT模型);%如果是多个标，那么每个标的noise_sigma2和x0都应该是不同的四、仿真结果运了三次，试图观察规律第次第次第三次感觉模型的曲线次仿真的差别有点，并且 向总是想找负向运动的，其他模型都是向着正向运动的。CVx