IMM卡尔曼滤波的matlab代码

### IMM 卡尔曼滤波 Matlab 实现

交互多模型（IMM, Interactive Multiple Model）算法结合多个不同的动态模型来提高目标跟踪性能。每个模型对应一种可能的目标运动模式，如匀速直线运动、转弯等。通过加权组合各个模型的状态估计结果，可以得到更精确的跟踪效果。

下面是一个简单的 IMM-Kalman 滤波器框架实现：

function [x_out, P_out] = imm_kf(z, dt)
% z 是观测向量；dt 时间间隔
    
    % 初始化参数
    N_models = 2;          % 假设有两个模型：常速度(CV) 和协调转向(CT)
    
    mu_in = ones(N_models,1)/N_models;
    x_cv = zeros(4,1);     % CV状态初始化位置和速度
    p_cv = eye(4)*1e3;
    x_ct = zeros(5,1);     % CT状态增加角速率分量
    p_ct = blkdiag(p_cv,[0]);
    
    Q_cv = diag([0.01, 0.01]);   % 过程噪声CV
    R = diag([10, 10]);         % 测量噪声
    
    F_cv = expm([zeros(2),eye(2);zeros(2)]*dt);
    H_cv = [eye(2),zeros(2)];
    
    G_cv = [[dt^2/2; dt], [dt^2/2; dt]];
    Q_cv = G_cv * Q_cv * G_cv';
    
    F_ct = expm([zeros(3),[0;0;1];zeros(1,4)]*dt);
    H_ct = [eye(2),zeros(2,3)];
    
    G_ct = [[dt^2/2; dt; 0],[dt^2/2; dt; 0]];
    Q_ct = G_ct*[Q_cv;0]*G_ct';

    c_jk = (mu_in'*ones(size(mu_in)))/(sum(mu_in)+length(mu_in)-1);

    for k=1:length(z(:,1))
        % 预测阶段
        [~, ~, cv_pred] = kalman_predict(x_cv,p_cv,F_cv,Q_cv,H_cv,R,z(k,:)',c_jk(1));
        [~, ~, ct_pred] = kalman_predict(x_ct,p_ct,F_ct,Q_ct,H_ct,R,z(k,:)',c_jk(2));

        % 更新混合概率
        llh_cv = log_likelihood(cv_pred(:,:,end),z(k,:)');
        llh_ct = log_likelihood(ct_pred(:,:,end),z(k,:)');

        mu_bar = softmax([llh_cv; llh_ct]);

        % 组合预测值作为最终输出
        x_mix = sum(cat(3,mu_bar(1)*cv_pred(:,:,end),...
                        mu_bar(2)*ct_pred(:,:,end)),3);
        
        % 更新阶段
        [~,~,cv_upd] = kalman_update(cv_pred(:,:,end),H_cv,z(k,:),R,c_jk(1));
        [~,~,ct_upd] = kalman_update(ct_pred(:,:,end),H_cv,z(k,:),R,c_jk(2));

        % 记录当前时刻的结果用于下一次迭代
        x_cv = squeeze(sum(cv_upd,3)); 
        p_cv = squeeze(sum(cell2mat(arrayfun(@(i){squeeze(cv_upd(:,:,i,2))}, ...
            1:size(cv_upd,3),'UniformOutput',false))));
            
        x_ct = squeeze(sum(ct_upd,3));
        p_ct = squeeze(sum(cell2mat(arrayfun(@(i){squeeze(ct_upd(:,:,i,2))}, ...
            1:size(ct_upd,3),'UniformOutput',false))));
                
        mu_in = mu_bar;

        % 存储输出
        if k==1
            x_out = x_mix;
            P_out = cov(x_mix);
        else
            x_out = cat(2,x_out,x_mix);
            P_out = cat(3,P_out,cov(x_mix));
        end
        
    end
end

function y = log_likelihood(pred,z)
    innov = z-pred(1:2,end)';
    S = pred(3:end-1,end)' + pred(end)^2;
    y = -0.5*(innov'/S*innov + length(innov)*log(2*pi*S));
end

function w = softmax(v)
    e_v = exp(v-max(v));
    w = e_v ./ sum(e_v);
end

此代码展示了如何利用两种不同类型的卡尔曼滤波器——即常速度模型和协调转角模型来进行联合估计[^1]。需要注意的是，这里仅提供了一个简化版本的例子，实际应用中还需要考虑更多细节，比如初始条件的选择以及过程噪声矩阵的设计等问题。