www.pudn.com > strong-tracking-filter.rar > initial_cov.m, change:2010-10-09,size:703b


function F=initial_cov(R1,R2,R3,T) 
% 根据前三个时刻的量测噪声计算初始协方差 
F=zeros(9,9); 
F(1:3,1:3)=com_matrix(R1(1,1),R2(1,1),R3(1,1),T);  
F(1:3,4:6)=com_matrix(R1(1,2),R2(1,2),R3(1,2),T);  
F(1:3,7:9)=com_matrix(R1(1,3),R2(1,3),R3(1,3),T);  
F(4:6,1:3)=com_matrix(R1(2,1),R2(2,1),R3(2,1),T);  
F(4:6,4:6)=com_matrix(R1(2,2),R2(2,2),R3(2,2),T);  
F(4:6,7:9)=com_matrix(R1(2,3),R2(2,1),R3(2,3),T);  
F(7:9,1:3)=com_matrix(R1(3,1),R2(3,1),R3(3,1),T);  
F(7:9,4:6)=com_matrix(R1(3,2),R2(3,2),R3(3,2),T);  
F(7:9,7:9)=com_matrix(R1(3,3),R2(3,3),R3(3,3),T);  
 
function P=com_matrix(r1,r2,r3,T) 
P=[r3, r3/T, r3/T^2; 
     r3/T, (r3+r2)/T^2,(r3+2*r2)/T^3; 
     r3/T^2,(r3+2*r2)/T^3,(r3+4*r2+r1)/T^4];