¡¾Â˲¨¡¿»ùÓÚ×î½üÁÚË㷨ʵÏÖ¶àÄ¿±êº½¼£¹ØÁª¸½matlab´úÂë

1 ÄÚÈݽéÉÜ

Õë¶Ô´¬²°×Ô¶¯Ê¶±ðϵͳ(AIS)Óë¸ßƵµØ²¨À×´ï(HFSWR)¶Ôº£ÉÏÄ¿±ê̽²âµÄÊý¾Ý,¶ÔÆäÄ¿±êº½¼£¹ØÁª¼¼Êõ½øÐÐÁËÑо¿. ½áºÏ×î½üÁÚÔ­ÔòÓ뿨¶ûÂüÂ˲¨Ëã·¨,Ìá³öÁËÒ»ÖÖ×î½üÁÚ-¿¨¶ûÂüÂ˲¨º½¼£¹ØÁªËã·¨.ͨ¹ý·ÂÕæÑéÖ¤½á¹û,±íÃ÷Á˸ú½ ¼£¹ØÁªËã·¨µÄÓÐЧÐÔÓëÕýÈ·ÐÔ.

2 ²¿·Ö´úÂë

function [s,P]=KF3(s_forward,P_forward,X,T,Q,C,H)

%¿¨¶ûÂüÂ˲¨

%²ÎÊý˵Ã÷

%? ? ? ?X--¹Û²âÊý¾ÝʸÁ¿

%? ? ? ?A--״̬¾ØÕó

%? ? ? ?Q--Çý¶¯ÔëÉùЭ·½²î

%? ? ? ?C--¹Û²âÔëÉùЭ·½²î

%? ? ? ?h--¹Û²â·½³Ì¾ä±ú

%? ? ? ?s--Êä³öÊý¾ÝʸÁ¿

%? ? ? ?s_foward--Ç°´ÎÊä³öʸÁ¿

%? ? ? ?M--Ç°´ÎÔ¤²â¾ØÕó

%״̬ת»»¾ØÕó

? ? AB=[1 T T^2/2 0 0 0;...

? ? ? ? 0 1 T? ? ?0 0 0;...

? ? ? ? 0 0 1? ? ?0 0 0;...

? ? ? ? 0 0 0? ? ?1 T T^2/2;...

? ? ? ? 0 0 0? ? ?0 1 T;...

? ? ? ? 0 0 0? ? ?0 0 1];?

A=[1,T,0,0;...

? ?0,1,0,0;...

? ?0,0,1,T;...

? ?0,0,0,1];

%Ò»²½¹ý³ÌÔ¤²â

s=s_forward;

s=A*s;

%×îСԤ²âMSE¾ØÕó

P=P_forward;

P=A*P*A'+Q;

%¿¨¶ûÂüÔöÒæ¾ØÕó

K=P*H'*inv(C+H*P*H');

%ÐÞÕý¹ý³ÌÔ¤²âºó，¼´µÃµ½Â˲¨Öµ

s=s+K*(X-H*s);

%×îС¹À¼ÆMSE¾ØÕó

P=P-K*H*P;

3 ÔËÐнá¹û

4 ²Î¿¼ÎÄÏ×

[1]Zheng Zhiqiang, ֣־ǿ, Liu Yongxin,µÈ. »ùÓÚ×î½üÁÚ-×ÔÊÊÓ¦Â˲¨AISÓëHFSWRº½¼£¹ØÁªËã·¨[C]// Öйúº½¿Õѧ»áÐÅÏ¢ÈںϷֻá. Öйúº½¿Õѧ»áÐÅÏ¢ÈںϷֻá, 2015.

²©Ö÷¼ò½é：Éó¤ÖÇÄÜÓÅ»¯Ëã·¨¡¢Éñ¾­ÍøÂçÔ¤²â¡¢ÐźŴ¦Àí¡¢Ôª°û×Ô¶¯»ú¡¢Í¼Ïñ´¦Àí¡¢Â·¾¶¹æ»®¡¢ÎÞÈË»ú¡¢À×´ïͨÐÅ¡¢ÎÞÏß´«¸ÐÆ÷µÈ¶àÖÖÁìÓòµÄMatlab·ÂÕæ，Ïà¹Ømatlab´úÂëÎÊÌâ¿É˽ÐŽ»Á÷¡£