前段时间不慎感染新冠，本来元旦好了，结果跑了个五公里。感觉心脏不行了，出现胸闷，气短，心痛的情况。保险起见又歇了几天，今天感觉还可以。所以把《卡尔曼滤波原理及应用-matlab仿真》的最后一篇交互多模型Kalman滤波学习一下，争取在过年前整理出来。话不多说，咱们一起来学习。
参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》这本书对kalman算法的解析很清晰，MATLAB程序很全，适合初学者（如有侵权，请联系删除(qq:1491967912)
1、背景：
在Kalman滤波算法中用到了状态转移方程和观测方程，被估计量随时间变化，他是一种动态估计。在目标跟踪中，不必知道目标的运动模型就能够实时地修正状态参量（位置、速度等信息），有良好的适应性。但是当目标运动运动变得复杂时（比如加速、减速等），仅仅用kalman滤波得不到理想的效果。这时就需要用自适应算法。交互多模型（IMM）是一种软切换算法，现在在机动目标领域得到广泛应用。该算法主要通过两个或更多的模型来描述工作过程中可能的状态，最后通过有效的加权融合进行系统状态估计，能够很好的克服单模型估计误差大的问题。
2、交互多模型Kalman滤波原理
IMM算法采用多个Kalman滤波器进行并行处理。每一个滤波器对应着不同的状态空间模型，不同的状态空间模型描述不同的目标运动模式，因此每一个滤波器对目标的状态估计是不同的。
IMM算法的基本思想：
1、在每个时刻，假设某个模型在现在时刻有效的前提下，通过混合前一时刻所有滤波器的状态估计值来获得与这个特定模型匹配的滤波器的初始条件；
2、然后对每一个并行实现正规滤波（预测和修正）步骤；
3、最后，以模型匹配似然函数为基础更新模型概率，并组合所有滤波器修正后的状态估计值（加权和）来得到状态估计。
因此IMM算法的估计结果是对不同模型所得估计的混合，而不是仅仅在每个时刻选择完全正确的模型来估计。下面我门通过一些公式来具体学习一下。
3、IMM算法步骤：
假定运动目标有r种运动状态，对应有r个运动模型（相当于有r个状态转移方程），假设第j个模型的目标状态方程为：
量测方程为：
公式中，Wj(k)是均值为0，Gj(k)是噪声驱动矩阵,协方差矩阵为Qj的白噪声序列。各个模型之间的转移是通过马尔科夫概率转移矩阵确定的，其中元素pij表示目标由第i个运动模型转移到第j个运动模型的概率。概率转移矩阵如下（关于马尔科夫概率转移矩阵的知识大家可以参考一下马尔科夫链与转移矩阵这篇文章，里面的例子非常生动形象）：
步骤1：输入交互（模型j）
由目标的状态估计
与上一步中每个滤波器的模型概率
得到混合估计
和协方差
,将混合估计作为当前循环的初始状态。具体参数计算如下：
模型j的预测概率（归一化常数）为：
模型i到模型j的混合概率为：
模型j的混合状态估计为：
模型j的混合协方差估计：
公式中的Pij是模型i到模型j的转移概率，μj(k-1)为模型j在k-1时刻的概率。
步骤2：kalman滤波（模型j）
以
、
和
作为输入进行Kalman滤波，来更新预测状态
和滤波协方差
。
预测：
预测误差协方差：
kalman增益：
滤波：
滤波协方差：
步骤3：模型概率更新
采用似然函数来更新模型概率来更新模型概率μj(k)，模型j的似然函数为：
公式中：
所以模型j的概率为：
公式14中，c为归一化常数，并且
步骤4：输出交互：
基于模型概率，对每个滤波器的估计结果加权合并，得到总的状态估计
和总的协方差估计
。
总的状态估计：
总的协方差估计：
所以，滤波器的总输出是多个滤波器估计结果的加权平均。权重即为该时刻正确描述目标运动的概率，简称为模型概率。
选取滤波器的目标运动模型，可以从下面三个方面考虑：
（1）选择一定个数的IMM滤波器，包括较为精准的模型和较为粗糙的模型。IMM滤波算法不仅描述了目标的连续运动状态，而且描述了目标的机动性。
（2）马尔科夫链状态转移概率的选取对IMM滤波器的性能有较大的影响。马尔科夫链状态转移概率矩阵实际上相当于模型状态方程的状态转移矩阵，它将直接影响模型误差和模型概率估计的准确性。一般情况下，当马尔科夫链状态转移概率呈现一定程度的模型性时，IMM滤波器能够更稳健地描述目标运动。
（3）IMM滤波算法具有模块化的特性 。当对目标的运动规律有较为清晰的了解时，滤波器可以选择能够比较精确的描述目标运动的模型。当无法预料目标的运动规律时，应该选择更加一般的模型，也就是说模型应该具有较强的鲁棒性。
4、交互多模型kalman滤波在目标跟踪中的应用
1、问题描述
假定有一个雷达对平面上运动的目标进行观测。目标在t=0~40s沿着y轴作匀速直线运动，运动速度为-15m/s，目标起始点为（2000m,10000m）；在t=400~600s向x轴方向做90°的慢速转弯，加速度为μx=μy=0.075m/s2,当慢转弯结束后加速度将会降到0；从t=610s开始做90度快速转弯，加速度为0.3m/s2；在660s结束快转弯，加速度降到0。雷达扫描周期T=2s，x和y独立进行观测，观测噪声的标准差均为100m。试建立雷达对目标的跟踪算法，并进行仿真分析。
2、IMM滤波器设计
针对上述描述的情况，采用三个模型：第一个模型是非机动模型，假定它的系统扰动噪声方差为0，即不考虑W的影响；第二个、第三个模型为机动模型，假定第二个模型的系统扰动噪声方差为Q=0.001I2*2，第三个模型系统扰动噪声方差为0.0144I2*2。控制模型转换的马尔科夫链的转移概率矩阵为：
在跟踪的初始阶段，首先采用常规kalman滤波（非机动模型）进行跟踪，从20次采样开始，采用三个模型的IMM算法。设定各个模型在此时刻的概率分别为μ1=0.8,μ2=0.1,μ3=0.1;
各个模型参数定义如下：
采用两点起始法（航迹起始算法之一，后面会针对航迹起始算法进行学习，有兴趣的可以关注了解一下），求得初始状态为：
定义滤波误差的均值为：
定义滤波误差的标准差为：
上式中，M为蒙特卡洛模拟次数k=1,2,...,N，N为采样次数。
5、算法程序：
clc
clear all
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 交互多模（IMM）kalman滤波在目标跟踪中的应用
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T=2; %雷达扫描周期，即采样周期
M=5; %仿真（滤波）次数
N=900/T; %总的采样点数
N1=400/T; %第一转弯处采样起点
N2=600/T; %第一匀速处采样起点
N3=610/T; %第二转弯处采样起点
N4=660/T; %第二匀速处采样起点
Delta=100; %测量噪声标准差
% 对真实轨迹和观测轨迹数据的初始化
Rx=zeros(N,1);
Ry=zeros(N,1);
Zx=zeros(N,M);
Zy=zeros(N,M);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 下面是产生的真实轨迹，可修改数据，改变目标的真实运动状态和轨迹
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%1-沿Y轴匀速运动
t=2:T:400;
x0=2000+0*t';
y0=10000-15*t';
%2-慢转弯
t=402:T:600;
x1=x0(N1)+0.075*((t'-400).^2)/2;
y1=y0(N1)-15*(t'-400)+0.075*((t'-400).^2)/2;
%3-匀速
t=602:T:610;
vx=0.075*(600-400);
x2=x1(N2-N1)+vx*(t'-600);
y2=y1(N2-N1)+0*t';
%4-快转弯
t=612:T:660;
x3=x2(N3-N2)+(vx*(t'-610)-0.3*((t'-610).^2)/2);
y3=y2(N3-N2)-0.3*((t'-610).^2)/2;
%5-匀速直线
t=662:T:900;
vy=-0.3*(660-610);
x4=x3(N4-N3)+0*t';
y4=y3(N4-N3)+vy*(t'-660);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%最后将上述所有轨迹整合成为一个列向量，即真实轨迹数据，Rx为Real-x,Ry为Real-y
Rx=[x0;x1;x2;x3;x4]; %不同时间段的x方向上的距离
Ry=[y0;y1;y2;y3;y4]; %不同时间段的y方向上的距离
%对每次蒙特卡洛仿真的滤波估计位置的初始化
Mt_Est_Px=zeros(M,N);
Mt_Est_Py=zeros(M,N);
%产生观测数据，要仿真M次，必须有M次的观测数据
nx=randn(N,M)*Delta; %产生观测噪声
ny=randn(N,M)*Delta;
Zx=Rx*ones(1,M)+nx; %真实值的基础上叠加噪声，就得到了计算机模型的观测值
Zy=Ry*ones(1,M)+ny;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for m=1:M
%滤波初始化
Mt_Est_Px(m,1)=Zx(1,m); %存放观测数据的初始数据x
Mt_Est_Py(m,1)=Zx(2,m); %存放观测数据的初始数据y
xn(1)=Zx(1,m);
xn(2)=Zx(2,m);
yn(1)=Zy(1,m);
yn(2)=Zy(2,m);
%非机动模型参数
phi=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
h=[1,0,0,0;0,0,1,0];
g=[T/2,0;1,0;0,T/2;0,1];
delta_w=1e-4;
Q=delta_w*diag([1 1]); %过程噪声方差
q=[Delta^2,0;0,Delta^2]; %观测噪声方差
vx=(Zx(2)-Zx(1,m))/T;
vy=(Zy(2)-Zy(1))/T;
%初始状态估计(采用的是两点起始法（航迹起始算法之一）)
x_est=[Zx(2,m);vx;Zy(2,m);vy];
p_est=[Delta^2,Delta^2/T,0,0;Delta^2/T,2*Delta^2/(T/2),0,0;
0,0,Delta^2,Delta^2/T;0,0,Delta^2/T,2*Delta^2/(T/2)];
Mt_Est_Px(m,2)=x_est(1); %存放状态估计的的初始数据x
Mt_Est_Py(m,2)=x_est(3); %存放状态估计的的初始数据y
%开始滤波
for r=3:N
z=[Zx(r,m);Zy(r,m)];
if(r<20)
%传统kalman滤波的预测阶段
x_pre=phi*x_est; %状态预测
p_pre=phi*p_est*phi'+g*Q*g'; %预测误差协方差
%传统kalman滤波的校正阶段
k=p_pre*h'*inv(h*p_pre*h'+q); %kalman增益
x_est=x_pre+k*(z-h*x_pre); %滤波
p_est=(eye(4)-k*h)*p_pre; %滤波的协方差
%记录采样点滤波数据
xn(r)=x_est(1);
yn(r)=x_est(3);
%记录第m次仿真滤波估计数据
Mt_Est_Px(m,r)=x_est(1);
Mt_Est_Py(m,r)=x_est(3);
else
if r==20 %由于从第20次采样开始采用三个模型的IMM算法，可以修改采样次数
%扩维处理
X_est=[x_est;0;0];
P_est=p_est;
P_est(6,6)=0;
for i=1:3
Xn_est{i,1}=X_est; %元胞
Pn_est{i,1}=P_est;
end
u=[0.8,0.1,0.1]; %模型概率初始化
end
%调用IMM算法
[X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,z,Delta,u);
xn(r)=X_est(1);
yn(r)=X_est(3);
Mt_Est_Px(m,r)=X_est(1);
Mt_Est_Py(m,r)=X_est(3);
end
end %到此结束一次滤波
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 滤波结果分析
err_x=zeros(N,1);
err_y=zeros(N,1);
delta_x=zeros(N,1);
delta_y=zeros(N,1);
%计算滤波的误差均值和标准差
for r=1:N
%估计误差均值，
ex=sum(Rx(r)-Mt_Est_Px(:,r));
ey=sum(Ry(r)-Mt_Est_Py(:,r));
err_x(r)=ex/M;
err_y(r)=ey/M;
eqx=sum((Rx(r)-Mt_Est_Px(:,r)).^2);
eqy=sum((Ry(r)-Mt_Est_Py(:,r)).^2);
%估计误差标准差
delta_x(r)=sqrt(abs(eqx/M-err_x(r)^2));
delta_y(r)=sqrt(abs(eqy/M-err_y(r)^2));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 绘图
%轨迹图（Rx,Ry）构成真实轨迹（Zx,Zy）为观测轨迹，若蒙特卡洛仿真次数很多，会导致观测轨迹看起来想一团，可以
%用蒙特卡洛均值代替。
figure(1)
plot(Rx,Ry,'k-');hold on;
plot(xn,yn,'r-.');hold on
plot(Zx,Zy,'g:');hold on
legend('真实轨迹','估计轨迹','观测样本');
%估计误差均值
figure(2)
subplot(2,1,1);
plot(err_x);
axis([1,N,-300,300]);
title('X方向估计误差均值');
subplot(2,1,2);
plot(err_y);
axis([1,N,-300,300]);
title('Y方向估计误差均值');
%估计误差标准差
figure(3)
subplot(2,1,1);
plot(delta_x);
title('X方向估计误差标准差');
subplot(2,1,2);
plot(delta_y);
title('Y方向估计误差标准差');
%子函数IMM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%子函数 IMM算法
%X_est,P_est返回第m次仿真第r个采样点的滤波结果
%Xn_est,Pn_est记录每个模型对应的第m次仿真第r个采样点的滤波结果
%u为模型概率
function [X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,Z,Delta,u)
%控制模型转换的马尔科夫链的转移概率矩阵
P=[0.95,0.025,0.025;0.025,0.95,0.025;0.025,0.025,0.95];
%所采用的三个模型参数，模型一为非机动，模型二、三均为机动模型（Q不同）
%% 状态转移矩阵
%模型1非机动模型
PHI{1,1}=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
PHI{1,1}(6,6)=0; %扩维处理
%模型2机动模型
PHI{2,1}=[1,T,0,0,T^2/2,0;0,1,0,0,T,0;0,0,1,T,0,T^2/2;
0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,1];
%模型3机动模型（模型2和3只是Q不相同）
PHI{3,1}=PHI{2,1};
%% 过程噪声驱动矩阵
%模型1
G{1,1}=[T/2,0;1,0;0,T/2;0,1];
G{1,1}(6,2)=0;
%模型2
G{2,1}=[T^2/4,0;T/2,0;0,T^2/4;0,T/2;1,0;0,1];
%模型3
G{3,1}=G{2,1};
%% 系统扰动噪声方差
%模型1
Q{1,1}=zeros(2);
%模型2
Q{2,1}=0.001*eye(2);
%模型3
Q{3,1}=0.0144*eye(2);
%% 其他参数配置
H=[1,0,0,0,0,0;0,0,1,0,0,0];
R=eye(2)*Delta^2; %观测噪声协方差矩阵
mu=zeros(3,3); %混合概率矩阵初始化
c_mean=zeros(1,3); %归一化常数
%% 三个模型对应的预测概率的归一化常数
for i=1:3
c_mean=c_mean+P(i,:)*u(i);
end
%% 模型i(1,2,3)到j(1,2,3)对应的混合概率
for i=1:3
mu(i,:)=P(i,:)*u(i)./c_mean;
end
%% IMM算法的步骤
%输入交互
for j=1:3
X0{j,1}=zeros(6,1); %模型j的混合状态轨迹初始化
P0{j,1}=zeros(6); %模型j的混合协方差估计初始化
%模型j的混合状态轨迹
for i=1:3
X0{j,1}=X0{j,1}+Xn_est{i,1}*mu(i,j);
end
%模型j的混合协方差估计
for i=1:3
P0{j,1}=P0{j,1}+mu(i,j)*(Pn_est{i,1}+(Xn_est{i,1}-X0{j,1})*(Xn_est{i,1}-X0{j,1})');
end
end
%模型的kalman滤波
for j=1:3
%观测预测
X_pre{j,1}=PHI{j,1}*X0{j,1};
%协方差预测
P_pre{j,1}=PHI{j,1}*P0{j,1}*PHI{j,1}'+G{j,1}*Q{j,1}*G{j,1}';
%计算kalman增益
K{j,1}=P_pre{j,1}*H'*inv(H*P_pre{j,1}*H'+R);
%状态更新
Xn_est{j,1}=X_pre{j,1}+K{j,1}*(Z-H*X_pre{j,1});
%协方差更新
Pn_est{j,1}=(eye(6)-K{j,1}*H)*P_pre{j,1};
end
%模型概率更新
a=zeros(1,3);
for j=1:3
v{j,1}=Z-H*X_pre{j,1}; %新息
s{j,1}=H*P_pre{j,1}*H'+R; %观测协方差矩阵
n=length(s{j,1})/2;
%观测相对于模型j的似然函数
a(1,j)=1/((2*pi)^n*sqrt(det(s{j,1})))*exp(-0.5*v{j,1}'*inv(s{j,1})*v{j,1});
end
c=sum(a.*c_mean); %模型的归一化常数
u=a.*c_mean./c; %模型的概率更新
%输出交互
Xn=zeros(6,1);
Pn=zeros(6);
for i=1:3
Xn=Xn+Xn_est{i,1}.*u(i);
end
for i=1:3
Pn=Pn+u(i).*(Pn_est{i,1}+(Xn_est{i,1}-Xn)*(Xn_est{i,1}-Xn)');
end
%返回滤波结果
X_est=Xn;
P_est=Pn;
6、仿真分析：
设定情景目标的实际轨迹、观测轨迹和一次滤波结果曲线如下图所示：从结果来看，滤波曲线基本上在真是轨迹附近，无论是直线运动还是转完运动。都能相对较好的跟上目标，达到一个较好的滤波结果。另外误差和标准差的图就不再分析了，有需要的可以自行跑一下程序看一下。
7、总结;
到这基本上把该书中得所用相关算法内容全部学完了，全书后面就剩了一个卡尔曼滤波的simulink仿真。后面要是有需要再学习一下吧。个人作为一个数据处理的小白，学完这本书让自己获得很多东西，个人建议如果有条件的话买一本静下心来细细研究，你会得到很多很多东西。到这关于《卡尔曼滤波原理及应用-------MATLAB仿真》的学习到此告一段落。后面要开始学习航迹起始和航迹关联的一些知识。如果大家感兴趣的话可以关注一下咱们一起学习。
