fget_next.m

IMM滤波的 matlab程序

function [find_sign,X,point_measurement]=fget_next(Z,X,X_jL,j,point_layer,point_measurement)
% find_sign找到合适的量测标识
[m0,m]=size(Z);
n=point_measurement(j)+1;

find_sign=0;%寻找成功标志
falsechoose_sign=0;%错误选择标志
while n<=m & find_sign==0
    %判断当前量测是否满足约束条件（和各层的X_jL不同）
    for k=1:point_layer
        if Z(n)==X_jL(k)
            falsechoose_sign=1;
        end
    end
    if falsechoose_sign==0
        find_sign=1;
        point_measurement(j)=n;%设置量测指针，当下层寻找失败后返回本层时从第point_measurement(j)+1量测开始寻找
        X(j)=Z(n);%可行事件中间输出变量,只更改j层数据，其他层数据不变
    else
        n=n+1;%若前一个量测不满足条件，搜索下一个量测
        falsechoose_sign=0;
    end
end
if n>m & find_sign==0
    point_measurement(j)=1;%当搜索完本行且没有找到合适的量测时量测指针赋起始位置
    X(j)=1;%因为下次要在上层搜索，该层要复位
end