卡尔曼滤波应用及其matlab实现

Github个人博客：https://joeyos.github.io

线性卡尔曼滤波

卡尔曼滤波在温度测量中的应用

X(k)=AX(k-1)+TW(k-1)
Z(k)=H*X(k)+V(k)

房间温度在25摄氏度左右，测量误差为正负0.5摄氏度，方差0.25，R=0.25。Q=0.01,A=1,T=1,H=1。

假定快时刻的温度值、测量值为23.9摄氏度，房间真实温度为24摄氏度，温度计在该时刻测量值为24.5摄氏度，偏差为0.4摄氏度。利用k-1时刻温度值测量第k时刻的温度，其预计偏差为:
P(k|k-1)=P(k-1)+Q=0.02
卡尔曼增益k=P(k|k-1) / (P(k|k-1) + R)=0.0741

X(k)=23.9+0.0741*(24.1-23.9)=23.915摄氏度。

k时刻的偏差为P(k)=(1-K*H)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。

matlab实现为：

% 程序说明：Kalman滤波用于温度测量的实例
function main
N=120;
CON=25;%房间温度在25摄氏度左右
Xexpect=CON*ones(1,N);
X=zeros(1,N);  
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1,N); 
X(1)=25.1;
P(1)=0.01;
Z(1)=24.9;
Xkf(1)=Z(1);
Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
F=1;
G=1;
H=1;
I=eye(1); 
%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
    X(k)=F*X(k-1)+G*W(k-1);  
    Z(k)=H*X(k)+V(k);
    X_pre=F*Xkf(k-1);           
    P_pre=F*P(k-1)*F'+Q;        
    Kg=P_pre*inv(H*P_pre*H'+R); 
    e=Z(k)-H*X_pre;            
    Xkf(k)=X_pre+Kg*e;         
    P(k)=(I-Kg*H)*P_pre;
end
%%%%%%%%%%%%%%%%
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for k=1:N
    Err_Messure(k)=abs(Z(k)-X(k));
    Err_Kalman(k)=abs(Xkf(k)-X(k));
end
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');         
xlabel('sample time');
%%%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在自由落体运动目标跟踪中的应用

%%%%%%%%%%%%%
% 卡尔曼滤波用于自由落体运动目标跟踪问题
%%%%%%%%%%%%%%
function main
N=1000; %仿真时间，时间序列总数
%噪声
Q=[0,0;0,0];%过程噪声方差为0，即下落过程忽略空气阻力
R=1; %观测噪声方差
W=sqrt(Q)*randn(2,N);%既然Q为0，即W=0
V=sqrt(R)*randn(1,N);%测量噪声V(k)
%系数矩阵
A=[1,1;0,1];%状态转移矩阵
B=[0.5;1];%控制量
U=-1;
H=[1,0];%观测矩阵
%初始化
X=zeros(2,N);%物体真实状态
X(:,1)=[95;1];%初始位移和速度
P0=[10,0;0,1];%初始误差
Z=zeros(1,N);
Z(1)=H*X(:,1);%初始观测值
Xkf=zeros(2,N);%卡尔曼估计状态初始化
Xkf(:,1)=X(:,1);
err_P=zeros(N,2);
err_P(1,1)=P0(1,1);
err_P(1,2)=P0(2,2);
I=eye(2); %二维系统
%%%%%%%%%%%%%
for k=2:N
    %物体下落，受状态方程的驱动
    X(:,k)=A*X(:,k-1)+B*U+W(k);
    %位移传感器对目标进行观测
    Z(k)=H*X(:,k)+V(k);
    %卡尔曼滤波
    X_pre=A*Xkf(:,k-1)+B*U;%状态预测 
    P_pre=A*P0*A'+Q;%协方差预测
    Kg=P_pre*H'*inv(H*P_pre*H'+R);%计算卡尔曼增益
    Xkf(:,k)=X_pre+Kg*(Z(k)-H*X_pre);%状态更新
    P0=(I-Kg*H)*P_pre;%方差更新
    %误差均方值
    err_P(k,1)=P0(1,1);
    err_P(k,2)=P0(2,2);
end
%误差计算
messure_err_x=zeros(1,N);%位移的测量误差
kalman_err_x=zeros(1,N);%卡尔曼估计的位移与真实位移之间的偏差
kalman_err_v=zeros(1,N);%卡尔曼估计的速度与真实速度之间的偏差
for k=1:N
    messure_err_x(k)=Z(k)-X(1,k);
    kalman_err_x(k)=Xkf(1,k)-X(1,k);
    kalman_err_v(k)=Xkf(2,k)-X(2,k);
end
%%%%%%%%%%%%%%%
%画图输出
%噪声图
figure
plot(V);
title('messure noise')
%位置偏差
figure
hold on,box on;
plot(messure_err_x,'-r.');%测量的位移误差
plot(kalman_err_x,'-g.');%卡尔曼估计位置误差
legend('测量误差','kalman估计误差')
figureplot(kalman_err_v);
title('速度误差')
figure
plot(err_P(:,1));
title('位移误差均方值')
figure
plot(err_P(:,1));
title('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在船舶GPS导航定位

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BauFQwsS-1575269126935)(https://img-blog.csdn.net/20180205210213419?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvemhhbmdxdWFuMjAxNQ==/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)]

%  Kalman滤波在船舶GPS导航定位系统中的应用
function main
clc;clear;
T=1;%雷达扫描周期
N=80/T;%总的采样次数
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
Z=zeros(2,N);%传感器对位置的观测
Z(:,1)=[X(1,1),X(3,1)];%观测初始化
delta_w=1e-2;%如果增大这个参数，目标真实轨迹就是曲线
Q=delta_w*diag([0.5,1,0.5,1]) ;%过程噪声均值
R=100*eye(2);%观测噪声均值
F=[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];%观测矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目标真实轨迹
    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1); %对目标观测
end
%卡尔曼滤波
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
P0=eye(4);%协方差阵初始化
for i=2:N
    Xn=F*Xkf(:,i-1);%预测
    P1=F*P0*F'+Q;%预测误差协方差
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end
%误差更新
for i=1:N
    Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
end
%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k');%真实轨迹
plot(Z(1,:),Z(2,:),'-b.');%观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-r+');%卡尔曼滤波轨迹
legend('真实轨迹','观测轨迹','滤波轨迹')
figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算欧式距离子函数
function dist=RMS(X1,X2);
if length(X2)<=2
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在视频目标跟踪中的应用

帧间差法目标检测

% 目标检测函数，这个函数主要完成将目标从背景中提取出来
function detect
clear,clc;
%计算背景图片数目
Imzero = zeros(240,320,3);
for i = 1:5
    %将图像文件读入矩阵Im
    Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
    Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);
%遍历所有图片
for i = 1 : 60
    %读取所有帧
    Im = (imread(['DATA/',int2str(i), '.jpg']));
    imshow(Im); %显示图像Im,图像对比度低
    Imwork = double(Im);
    %检测目标
    [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);
    if flag==0 %没检测到目标，继续下一帧图像
        continue
    end
    hold on
    for c = -0.9*radius: radius/20 : 0.9*radius
        r = sqrt(radius^2-c^2);
        plot(cc(i)+c,cr(i)+r,'g.')
        plot(cc(i)+c,cr(i)-r,'g.')
    end
    pause(0.02)
end
%目标中心的位置，也就是目标的x，y坐标
figure
plot(cr,'-g*')
hold on
plot(cc,'-r*')

% 提取目标区域的中心和能包含目标的最大半径
function [cc,cr,radius,flag]=extractball(Imwork,Imback,index)
%初始化目标区域中心的坐标，半径
cc = 0;
cr = 0;
radius = 0;
flag = 0;
[MR,MC,Dim] = size(Imback);
%除去背景，找到最大的不同区域，即目标区域
fore = zeros(MR,MC);    
%背景相减，得到目标      
fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) ...
    | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) ...
    | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10);
%图像腐蚀，除去微小的白噪声点
%bwmorph该函数的功能是：提取二进制图像的轮廓
foremm = bwmorph(fore,'erode',2);%“2”为次数
%选取最大的目标
labeled = bwlabel(foremm,4);%黑背景中甄别有多少白块块，4-连通
%labeled是标记矩阵，图像分割后对不同的区域进行不同的标记
stats = regionprops(labeled,['basic']);
[N,W] = size(stats);
if N < 1
    return%一个目标区域也没检测到就返回
end
%在N个区域中，冒泡算法排序
id = zeros(N);
for i = 1 : N
    id(i) = i;
end
for i = 1 : N-1
    for j = i+1 : N
        if stats(i).Area < stats(j).Area
            tmp = stats(i);
            stats(i) = stats(j);
            stats(j) = tmp;
            tmp = id(i);
            id(i) = id(j);
            id(j) = tmp;
        end
    end
end
%确保至少有一个较大的区域（最大区域面积要大于100）
if stats(1).Area < 100
    return
end
selected = (labeled==id(1));
%计算最大区域的中心和半径
centroid = stats(1).Centroid;
radius = sqrt(stats(1).Area/pi);
cc = centroid(1);
cr = centroid(2);
flag = 1;
return

视频目标跟踪

%  kalman滤波用户检测视频中的目标，并对目标跟踪
function kalman_for_vedio_tracking
clear,clc
% 计算图像背景
Imzero = zeros(240,320,3);
for i = 1:5
Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);

% Kalman 滤波器初始化
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
P = 100*eye(4);
dt=1;  % 采样时间，也就是图像帧时间
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
g = 6; % pixels^2/time step
Bu = [0,0,0,g]';
kfinit=0;           % kalman增益初始化
x=zeros(100,4);     % 目标状态初始化

% 检测视频中每一帧图像
for i = 1 : 60
  % 读取图像
  Im = (imread(['DATA/',int2str(i), '.jpg'])); 
  imshow(Im)
  imshow(Im)
  Imwork = double(Im);

  % 检测目标（目标是一个球）
  [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);
  if flag==0
    continue
  end

  hold on
    for c = -1*radius: radius/20 : 1*radius
      r = sqrt(radius^2-c^2);
      plot(cc(i)+c,cr(i)+r,'g.')
      plot(cc(i)+c,cr(i)-r,'g.')
    end
  % Kalman update
i
  if kfinit==0
    xp = [MC/2,MR/2,0,0]'
  else
    xp=A*x(i-1,:)' + Bu
  end
  kfinit=1;
  PP = A*P*A' + Q
  K = PP*H'*inv(H*PP*H'+R)
  x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
  x(i,:)
  [cc(i),cr(i)]
  P = (eye(4)-K*H)*PP

  hold on
    for c = -1*radius: radius/20 : 1*radius
      r = sqrt(radius^2-c^2);
      plot(x(i,1)+c,x(i,2)+r,'r.')
      plot(x(i,1)+c,x(i,2)-r,'r.')
    end
      pause(0.3)
end

% show positions
  figure
  plot(cc,'r*')
  hold on
  plot(cr,'g*')
%end

%estimate image noise (R) from stationary ball
  posn = [cc(55:60)',cr(55:60)'];
  mp = mean(posn);
  diffp = posn - ones(6,1)*mp;
  Rnew = (diffp'*diffp)/5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 提取目标区域的中心和能包含目标的最大半径
function [cc,cr,radius,flag]=extractball(Imwork,Imback,index)
cc = 0;
cr = 0;
radius = 0;
flag = 0;
[MR,MC,Dim] = size(Imback);
fore = zeros(MR,MC);          
fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) ...
    | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) ...
    | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10);
foremm = bwmorph(fore,'erode',2);
labeled = bwlabel(foremm,4);
stats = regionprops(labeled,['basic']);
[N,W] = size(stats);
if N < 1
    return
end
id = zeros(N);
for i = 1 : N
    id(i) = i;
end
for i = 1 : N-1
    for j = i+1 : N
        if stats(i).Area < stats(j).Area
            tmp = stats(i);
            stats(i) = stats(j);
            stats(j) = tmp;
            tmp = id(i);
            id(i) = id(j);
            id(j) = tmp;
        end
    end
end
if stats(1).Area < 100
    return
end
selected = (labeled==id(1));
centroid = stats(1).Centroid;
radius = sqrt(stats(1).Area/pi);
cc = centroid(1);
cr = centroid(2);
flag = 1;
return

扩展卡尔曼滤波

对于非线性系统滤波问题，常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题，其中应用最广泛的方法是扩展卡尔曼滤波（EKF）。扩展卡尔曼滤波建立在线性卡尔曼滤波的基础之上。其核心思想是，对一般的非线性系统，首先围绕滤波值Xk将非线性函数f和h展开成泰勒级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成对目标的滤波估计等处理。

EKF的优点是不必预先计算标称轨迹，过程噪声W(k)和观测噪声V(k)均为0,时非线性方程的解，但它只能在滤波误差及一步预测误差较小时才能使用。

简单非线性系统的扩展卡尔曼滤波器

%  函数功能：标量非线性系统扩展Kalman滤波问题
%  状态函数：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k) +w(k)
%  观测方程：Z（k）=X(k)^2/20 +v(k)
function EKF_for_One_Div_UnLine_System
T=50;%总时间
Q=10;%Q的值改变，观察不同Q值时滤波结果
R=1;%测量噪声
%产生过程噪声
w=sqrt(Q)*randn(1,T);
%产生观测噪声
v=sqrt(R)*randn(1,T);
%状态方程
x=zeros(1,T);
x(1)=0.1;
y=zeros(1,T);
y(1)=x(1)^2/20+v(1);
for k=2:T
    x(k)=0.5*x(k-1)+2.5*x(k-1)/(1+x(k-1)^2)+8*cos(1.2*k)+w(k-1);
    y(k)=x(k)^2/20+v(k);
end
%EKF滤波算法
Xekf=zeros(1,T);
Xekf(1)=x(1);
Yekf=zeros(1,T);
Yekf(1)=y(1);
P0=eye(1);
for k=2:T
    %状态预测
    Xn=0.5*Xekf(k-1)+2.5*Xekf(k-1)/(1+Xekf(k-1)^2)+8*cos(1.2*k);
    %观测预测
    Zn=Xn^2/20;
    %求状态矩阵F
    F=0.5+2.5 *(1-Xn^2)/(1+Xn^2)^2;
    %求观测矩阵
    H=Xn/10;
    %协方差预测
    P=F*P0*F'+Q;
    %求卡尔曼增益     
    K=P*H'*inv(H*P*H'+R);
    %状态更新
    Xekf(k)=Xn+K*(y(k)-Zn);
    %协方差阵更新
    P0=(eye(1)-K*H)*P;
end
%误差分析
Xstd=zeros(1,T);
for k=1:T
    Xstd(k)=abs( Xekf(k)-x(k) );
end
%画图
figure
hold on;box on;
plot(x,'-ko','MarkerFace','g');
plot(Xekf,'-ks','MarkerFace','b');
%误差分析
figure
hold on;box on;
plot(Xstd,'-ko','MarkerFace','g');

真实值与估计值对比：

滤波误差：

EKF在目标跟踪中的应用

基于距离的目标跟踪算法Matlab程序

%  扩展Kalman滤波在目标跟踪中的应用
function EKF_For_TargetTracking
clc;clear;
T=1;%雷达扫描周期
N=60/T;%总的采样次数
X=zeros(4,N);%目标初始位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
Z=zeros(1,N);%传感器对位置的观测
delta_w=1e-3;%如果增大这个参数，目标真实轨迹就是曲线
Q=delta_w*diag([0.5,1]);%过程噪声方差
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
R=5;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=200;%观测站的位置，可以设为其他值
y0=300; 
Xstation=[x0,y0];
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹 
end
for t=1:N
    Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%对目标观测
end
%EKF滤波
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);%卡尔曼滤波状态初始化
P0=eye(4);%协方差阵初始化
for i=2:N
    Xn=F*Xekf(:,i-1);%预测
    P1=F*P0*F'+G*Q*G';%预测误差协方差
    dd=Dist(Xn,Xstation);%观测预测
    %求雅克比矩阵H
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%即为所求一阶近似
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end
%误差分析
for i=1:N
    Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%滤波后的误差
end
%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');%真实轨迹
plot(Xekf(1,:),Xekf(3,:),'-r+');%扩展卡尔曼滤波轨迹
legend('真实轨迹','EKF轨迹')
figure
hold on; box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')

%子函数，求两点间的距离
function d=Dist(X1,X2);
if length(X2)<=2
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end

基于EKF的纯方位目标跟踪算法

%  扩展Kalman滤波在纯方位目标跟踪中的应用实例
function EKF_angle
clc;clear;
T=1;%雷达扫描周期
N=40/T;%总的采样次数
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[0,2,1400,-10];%目标初始位置、速度
Z=zeros(1,N); %传感器对位置的观测
delta_w=1e-4;%如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([1,1]) ;%过程噪声均值
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
R=0.1*pi/180;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=0;%观测站的位置，可以设为其他值
y0=1000; 
Xstation=[x0;y0];

w=sqrtm(R)*randn(1,N);%均值为0，方差为1的高斯噪声
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹
end
for t=1:N
    Z(t)=hfun(X(:,t),Xstation)+w(t);%目标观测
    %对sqrtm(R)*w(t)转化为角度sqrtm(R)*w(t)/pi*180可以看出噪声的大小
end
%EKF滤波
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);%卡尔曼滤波状态初始化
P0=eye(4);%协方差阵初始化
for i=2:N
    Xn=F*Xekf(:,i-1);%预测
    P1=F*P0*F'+G*Q*G';%预测误差协方差
    dd=hfun(Xn,Xstation);%观测预测
    %求雅克比矩阵H
    D=Dist(Xn,Xstation);
    H=[-(Xn(3,1)-y0)/D,0,(Xn(1,1)-x0)/D,0];%即为所求一阶近似
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end
%误差分析
for i=1:N
  Err_KalmanFilter(i)=sqrt(Dist(X(:,i),Xekf(:,i)));
end
%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');%真实轨迹
plot(Xekf(1,:),Xekf(3,:),'-r+');%扩展卡尔曼滤波轨迹
legend('真实轨迹','EKF轨迹')
figure
hold on; box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
figure 
hold on;box on;
plot(Z/pi*180,'-r.','MarkerFace','r');%真实角度值
plot(Z/pi*180+w/pi*180,'-ko','MarkerFace','g');%受噪声污染的观测值
legend('真实角度','观测角度');
function cita=hfun(X1,X0) %需要注意各个象限角度的变化
if X1(3,1)-X0(2,1)>=0
    if X1(1,1)-X0(1,1)>0
        cita=atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0
        cita=pi/2;
    else
        cita=pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
else
    if X1(1,1)-X0(1,1)>0
        cita=3*pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    elseif X1(1,1)-X0(1,1)==0
        cita=3*pi/2;
    else
        cita=pi+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
    end
end
function d=Dist(X1,X2);
if length(X2)<=2
    d=( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    d=( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end

跟踪轨迹

跟踪误差RMS

观测值和真实值的对比

EKF在三维纯方位寻的导弹制导中的应用

%  程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数
%  状态方程： x(t)=Ax(t-1)+Bu(t-1)+w(t）
function main
delta_t=0.01;%测量周期，采样周期
longa=10000;%机动时间常数的倒数，即机动频率
tf=3.7;   
T=tf/delta_t;%时间长度3.7s，一共采样T=370次
%状态转移矩阵F
F=[eye(3),delta_t*eye(3),(exp(-1*longa*delta_t)+...
   longa*delta_t-1)/longa^2*eye(3);
    zeros(3),eye(3),(1-exp(-1*longa*delta_t))/longa*eye(3);
    zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3)]; 
%控制量驱动矩阵gama
G=[-1*0.5*delta_t^2*eye(3);-1*delta_t*eye(3);zeros(3)]; 
N=3; %导航比（制导率）
for i=1:50	%做50次蒙特卡洛仿真
    x=zeros(9,T);
    x(:,1)=[3500,1500,1000,-1100,-150,-50,0,0,0]';%初始状态X(0)
    ex=zeros(9,T);   
    ex(:,1)=[3000,1200,960,-800,-100,-100,0,0,0]';%滤波器状态Xekf(0)
    cigema=sqrt(0.1);  
    w=[zeros(6,T);cigema*randn(3,T)];%过程噪声
    Q=[zeros(6),zeros(6,3);zeros(3,6),cigema^2*eye(3)];  
    z=zeros(2,T);%观测值
    z(:,1)=[atan( x(2,1)/sqrt(x(1,1)^2+x(3,1)^2) ), atan(-1*x(3,1)/x(1,1))]';
    v=zeros(2,T);%观测噪声
    for k=2:T-3
        tgo=tf-k*0.01+0.0000000000000001;
        c1=N/tgo^2;%制导率的系数
        c2=N/tgo;%制导率的系数
        c3=N*(exp(-longa*tgo)+longa*tgo-1)/(longa*tgo)^2;%制导率的系数
        %X、Y、Z三个方向的导弹加速度
        u(1,k-1)=[c1,c2,c3]*[x(1,k-1),x(4,k-1),x(7,k-1)]';
        u(2,k-1)=[c1,c2,c3]*[x(2,k-1),x(5,k-1),x(8,k-1)]';
        u(3,k-1)=[c1,c2,c3]*[x(3,k-1),x(6,k-1),x(9,k-1)]';
        x(:,k)=F*x(:,k-1)+G*u(:,k-1)+w(:,k-1);%目标状态方程
        d=sqrt(x(1,k)^2+x(2,k)^2+x(3,k)^2);
        D=[d,0;0,d];
        R=inv(D)*0.1*eye(2)*inv(D)';%观测噪声方差
        v(:,k)=sqrtm(R)*randn(2,1);%观测噪声模拟
        %目标观测方程
        z(:,k)=[atan( x(2,k)/sqrt(x(1,k)^2+x(3,k)^2) ), ...
            atan(-1*x(3,k)/x(1,k))]'+v(:,k);  
    end
    %根据观测值开始滤波
    P0=[10^4*eye(6),zeros(6,3);zeros(3,6),10^2*eye(3)];%协方差初始化
    eP0=P0;
    stop=0.5/0.01;
    span=1/0.01;
    for k=2:T-3
        dd=sqrt(ex(1,k-1)^2+ex(2,k-1)^2+ex(3,k-1)^2);
        DD=[dd,0;0,dd];
        RR=0.1*eye(2)/(DD*DD');
        tgo=tf-k*0.01+0.0000000000000001;
        c1=N/tgo^2;
        c2=N/tgo;
        c3=N*(exp(-longa*tgo)+longa*tgo-1)/(longa*tgo)^2;
        u(1,k-1)=[c1,c2,c3]*[ex(1,k-1),ex(4,k-1),ex(7,k-1)]';
        u(2,k-1)=[c1,c2,c3]*[ex(2,k-1),ex(5,k-1),ex(8,k-1)]';
        u(3,k-1)=[c1,c2,c3]*[ex(3,k-1),ex(6,k-1),ex(9,k-1)]';
        %调用扩展卡尔曼算法子函数
        [ex(:,k),eP0]=ekf(F,G,Q,RR,eP0,u(:,k-1),z(:,k),ex(:,k-1));
    end
    for t=1:T-3 %求每个时间点误差的平方
        Ep_ekfx(i,t)=sqrt((ex(1,t)-x(1,t))^2);
        Ep_ekfy(i,t)=sqrt((ex(2,t)-x(2,t))^2);
        Ep_ekfz(i,t)=sqrt((ex(3,t)-x(3,t))^2);
        Ep_ekf(i,t)=sqrt( (ex(1,t)-x(1,t))^2+(ex(2,t)-x(2,t))^2+(ex(3,t)-x(3,t))^2 );
        Ev_ekf(i,t)=sqrt( (ex(4,t)-x(4,t))^2+(ex(5,t)-x(5,t))^2+(ex(6,t)-x(6,t))^2 );
        Ea_ekf(i,t)=sqrt( (ex(7,t)-x(7,t))^2+(ex(8,t)-x(8,t))^2+(ex(9,t)-x(9,t))^2 );
    end

    for t=1:T-3 %求误差的均值，即RMS
        error_x(t)=mean(Ep_ekfx(:,t));
        error_y(t)=mean(Ep_ekfy(:,t));
        error_z(t)=mean(Ep_ekfz(:,t));
        error_r(t)=mean(Ep_ekf(:,t));
        error_v(t)=mean(Ev_ekf(:,t));
        error_a(t)=mean(Ea_ekf(:,t));
    end
end
t=0.01:0.01:3.67;
%轨迹图
figure
hold on;box on;grid on;
plot3(x(1,:),x(2,:),x(3,:),'-k.')
plot3(ex(1,:),ex(2,:),ex(3,:),'-r.','MarkerFace','r')
legend('real','ekf');
view(3)
title('position')
%位置偏差图
figure
hold on;box on;grid on;
plot(t,error_r,'b');
xlabel('飞行时间/s');
ylabel('弹目相对距离估计误差/m');
%加速度偏差图
figure
hold on;box on;grid on;
plot(t,error_v,'b');
xlabel('飞行时间/s');
ylabel('速度估计误差m/s');

%子函数
%ex为扩展卡尔曼估计得到的状态
function [ex,P0]=ekf(F,G,Q,R,P0,u,z,ex)
%状态预测
Xn=F*ex+G*u;
%观测预测
Zn=[atan( Xn(2)/sqrt(Xn(1)^2+Xn(3)^2) ),atan(-1*Xn(3)/Xn(1))]';
%协方差阵预测
P=F*P0*F'+Q;
%计算线性化的H矩阵
dh1_dx=-1*Xn(1)*Xn(2)/(Xn(1)^2+Xn(2)^2+Xn(3)^2)/sqrt(Xn(1)^2+Xn(3)^2);
dh1_dy=sqrt(Xn(1)^2+Xn(3)^2)/(Xn(1)^2+Xn(2)^2+Xn(3)^2);
dh1_dz=-1*Xn(2)*Xn(3)/(Xn(1)^2+Xn(2)^2+Xn(3)^2)/sqrt(Xn(1)^2+Xn(3)^2);
dh2_dx=Xn(3)/(Xn(1)^2+Xn(3)^2);
dh2_dy=0;
dh2_dz=-1*Xn(1)/(Xn(1)^2+Xn(3)^2);
H=[dh1_dx,dh1_dy,dh1_dz,0,0,0,0,0,0;dh2_dx,dh2_dy,dh2_dz,0,0,0,0,0,0];
%卡尔曼增益
K=P*H'/(H*P*H'+R);
%状态更新
ex=Xn+K*(z-Zn);
%协方差阵更新
P0=(eye(9)-K*H)*P;

寻的导弹EKF跟踪位置偏差

寻的导弹EKF跟踪速度偏差

寻的导弹EKF跟踪加速度偏差

无迹卡尔曼滤波

前面讨论的扩展卡尔曼滤波算法是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项，这样不可避免地引入线性化误差。如果线性化假设不成立，采用这种算法则会导致滤波器性能下降以至于造成发散。另外，在一般情况下计算系统状态方程和观测方程的Jacobian矩阵是不易实现的，增加了算法的计算复杂度。

无迹卡尔曼滤波UKF摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹变换UT来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对Jacobian矩阵进行求导。UKF没有把高阶项忽略，因此对于非线性分布的娃统计量有较高的计算精度，有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷。

无迹卡尔曼滤波在单观测站目标跟踪中的应用

%  无迹Kalman滤波在目标跟踪中的应用
function UKF
clc;clear;
T=1; %雷达扫描周期
N=60/T; %总的采样次数
X=zeros(4,N);%目标真实位置，速度
X(:,1)=[-100,2,200,20]; %目标初始位置、速度
Z=zeros(1,N);  %传感器对位置的观测
delta_w=1e-3; %如果增大这个参数，目标真实轨迹就是曲线
Q=delta_w*diag([0.5,1]) ;%过程噪声均值
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
R=5;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=200; %观测站的位置，可以设为其他值
y0=300;
Xstation=[x0,y0]; %雷达站的位置

w=sqrtm(R)*randn(1,N);
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标观测
end
%UKF滤波，UT变换
for t=1:N
    Z(t)=Dist(X(:,t),Xstation)+w(t);
end
L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2*L+1
    Wm(j)=1/(2*(L+ramda));
    Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值计算

Xukf=zeros(4,N);
Xukf(:,1)=X(:,1);%无迹卡尔曼滤波状态初始化
P0=eye(4);%协方差阵初始化
for t=2:N
    xestimate= Xukf(:,t-1);
    P=P0;
    %第一步：获得一组Sigma点集
    cho=(chol(P*(L+ramda)))';
    for k=1:L
        xgamaP1(:,k)=xestimate+cho(:,k);
        xgamaP2(:,k)=xestimate-cho(:,k);
    end
    Xsigma=[xestimate,xgamaP1,xgamaP2];
    %第二步：对Sigma点集进行一步预测
    Xsigmapre=F*Xsigma;
    %第三步：利用第二步的结果计算均值和协方差
    Xpred=zeros(4,1);%均值
    for k=1:2*L+1
        Xpred=Xpred+Wm(k)*Xsigmapre(:,k);
    end
    Ppred=zeros(4,4);%协方差阵预测
    for k=1:2*L+1
        Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
    end
    Ppred=Ppred+G*Q*G';
    %第四步：根据预测值，再一次使用UT变换，得到新的sigma点集
    chor=(chol((L+ramda)*Ppred))';
    for k=1:L
        XaugsigmaP1(:,k)=Xpred+chor(:,k);
        XaugsigmaP2(:,k)=Xpred-chor(:,k);
    end
    Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
    %第五步：观测预测
    for k=1:2*L+1%观测预测
        Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);
    end
    %第六步：计算观测预测均值和协方差
    Zpred=0;%观测预测的均值
    for k=1:2*L+1
        Zpred=Zpred+Wm(k)*Zsigmapre(1,k);
    end
    Pzz=0;
    for k=1:2*L+1
        Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
    end
    Pzz=Pzz+R;%得到协方差Pzz

    Pxz=zeros(4,1);
    for k=1:2*L+1
        Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
    end
    %第七步：计算卡尔曼增益
    K=Pxz*inv(Pzz);
    %第八步：状态和方差更新
    xestimate=Xpred+K*(Z(t)-Zpred);%状态更新
    P=Ppred-K*Pzz*K';%方差更新
    P0=P;
    Xukf(:,t)=xestimate;
end
%误差分析
for i=1:N
    Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));%滤波后的误差
end
%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');%真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-r+');%无迹卡尔曼滤波轨迹
legend('真实轨迹','UKF轨迹')
figure
hold on; box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')

%子函数：求两点间的距离
function d=Dist(X1,X2)
if length(X2)<=2
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%观测子函数：观测距离
function [y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);

跟踪轨迹

误差曲线

UKF在匀加速直线运动目标跟踪中的应用

%  功能说明： UKF在目标跟踪中的应用
%  参数说明： 1、状态6维，x方向的位置、速度、加速度；
%               y方向的位置、速度、加速度；
%            2、观测信息为距离和角度；
function ukf_for_track_6_div_system
n=6;%状态维数
t=0.5;%采用点数
Q=[1 0 0 0 0 0;
    0 1 0 0 0 0;
    0 0 0.01 0 0 0;
    0 0 0 0.01 0 0;
    0 0 0 0 0.0001 0;
    0 0 0 0 0 0.0001];%过程噪声协方差阵
R = [100 0;
    0 0.001^2];%量测噪声协方差阵
%状态方程
f=@(x)[x(1)+t*x(3)+0.5*t^2*x(5);x(2)+t*x(4)+0.5*t^2*x(6);...
    x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)];
%x1为X轴位置，x2为Y轴位置，x3、x4分别是X、Y轴的速度，x5、x6为X、Y两方向的加速度
%观测方程
h=@(x)[sqrt(x(1)^2+x(2)^2);atan(x(2)/x(1))];
s=[1000;5000;10;50;2;-4];
x0=s+sqrtm(Q)*randn(n,1);%初始化状态
P0 =[100 0 0 0 0 0;
    0 100 0 0 0 0;
    0 0 1 0 0 0;
    0 0 0 1 0 0;
    0 0 0 0 0.1 0;
    0 0 0 0 0 0.1];%初始化协方差
N=50;%总仿真时间步数，即总时间
Xukf = zeros(n,N);%UKF滤波状态初始化
X = zeros(n,N);%真实状态
Z = zeros(2,N);%测量值
for i=1:N
    X(:,i)= f(s)+sqrtm(Q)*randn(6,1);%模拟，产生目标运动真实轨迹
    s = X(:,i);
end
ux=x0;%ux为中间变量
for k=1:N
    Z(:,k)= h(X(:,k)) + sqrtm(R)*randn(2,1);%测量值，保存观测
    [Xukf(:,k), P0] = ukf(f,ux,P0,h,Z(:,k),Q,R);%调用ukf滤波算法
    ux=Xukf(:,k);
end
%跟踪误差分析
%这里只分析位置误差，速度、加速度误差分析在此略
for k=1:N
    RMS(k)=sqrt( (X(1,k)-Xukf(1,k))^2+(X(2,k)-Xukf(2,k))^2 );
end
%画图
figure
t=1:N;
hold on;box on;
plot( X(1,t),X(2,t), 'k-')
plot(Z(1,t).*cos(Z(2,t)),Z(1,t).*sin(Z(2,t)),'-b.')
plot(Xukf(1,t),Xukf(2,t),'-r.')
legend('实际值','测量值','ukf估计值');
xlabel('x方向位置/米')
ylabel('y方向位置/米')
%误差分析图
figure
box on;
plot(RMS,'-ko','MarkerFace','r')
xlabel('t/秒')
ylabel('偏差/米')

%UKF子函数
function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R)
%非线性系统中UKF算法
L=numel(X);%状态维数
m=numel(Z);%观测维数
alpha=1e-2;%默认系数，参看UT变换，下同
ki=0;%默认系数
beta=2;%默认系数
lambda=alpha^2*(L+ki)-L;%默认系数
c=L+lambda;%默认系数
Wm=[lambda/c 0.5/c+zeros(1,2*L)];%权值
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);%权值
c=sqrt(c);
%第一步，获取一组sigma点集
%sigma点集，在状态X附近的点集，X是6*13矩阵，每列为1样本
Xsigmaset=sigmas(X,P,c); 
%第二、第三、四步，对sigma点集进行一步预测，得到均值XImeans和方差P1和新sigma点集X1
%对状态UT变换
[X1means,X1,P1,X2]=ut(ffun,Xsigmaset,Wm,Wc,L,Q);   

%第五、六步，得到观测预测，Z1为X1集合的预测，Zpre为Z1的均值。
%Pzz为协方差
[Zpre,Z1,Pzz,Z2]=ut(hfun,X1,Wm,Wc,m,R);%对观测UT变换
Pxz=X2*diag(Wc)*Z2';%协方差Pxz
%第七步，计算卡尔曼增益
K=Pxz*inv(Pzz);
%第八步，状态和方差更新
X=X1means+K*(Z-Zpre);%状态更新
P=P1-K*Pxz';%协方差更新

%UT变换子函数
% 输入：fun为函数句柄，Xsigma为样本集，Wm和Wc为权值，n为状态维数（n=6）,COV为方差
% 输出：Xmeans为均值
function [Xmeans,Xsigma_pre,P,Xdiv]=ut(fun,Xsigma,Wm,Wc,n,COV)
LL=size(Xsigma,2);%得到Xsigma样本个数
Xmeans=zeros(n,1);%均值
Xsigma_pre=zeros(n,LL);
for k=1:LL
    Xsigma_pre(:,k)=fun(Xsigma(:,k));%一步预测
    Xmeans=Xmeans+Wm(k)*Xsigma_pre(:,k);
end
Xdiv=Xsigma_pre-Xmeans(:,ones(1,LL));%预测减去均值
P=Xdiv*diag(Wc)*Xdiv'+COV;%协方差

%产生Sigma点集函数
function Xset=sigmas(X,P,c)
A = c*chol(P)';%Cholesky分解
Y = X(:,ones(1,numel(X)));
Xset = [X Y+A Y-A];

运动轨迹

跟踪误差

交互多模型卡尔曼滤波

在卡尔曼滤波算法中用到了状态转移方程和观测方程，被估计量随时间变化，它是一种动态估计。在目标跟踪中，不必知道目标的运动模型就能够实时的修正目标的状态参量（位置、速度等），具有良好的适应性。但是当目标实施机动时，仅采用基本的卡尔曼滤波算法往往得不到理想的结果。这时需要采用自适应算法。交互多模型IMM是一种软切换算法，目前在机动目标跟踪领域中得到了广泛的应用。IMM算法使用两个或更多的模型来描述工作过程中可能的状态，最后通过有效的加权融合进行系统状态估计，很好地克服了单模型估计误差较大的问题。

%   交互多模Kalman滤波在目标跟踪中的应用
%
function ImmKalman
clear;
T=2;%雷达扫描周期，即采样周期
M=50;%滤波次数
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];
Ry=[y0;y1;y2;y3;y4];
% 对每次蒙特卡洛仿真的滤波估计位置的初始化
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);%初始数据
    Mt_Est_Py(m,1)=Zx(2,m);
    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];
    q=[Delta^2,0;0,Delta^2];
    vx=(Zx(2)-Zx(1,m))/2;
    vy=(Zy(2)-Zy(1,m))/2;
    %初始状态估计
    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);
    Mt_Est_Py(m,2)=x_est(3);
    %滤波开始
    for r=3:N
        z=[Zx(r,m);Zy(r,m)];
        if r<20
            x_pre=phi*x_est;%预测
            p_pre=phi*p_est*phi';%预测误差协方差
            k=p_pre*h'*inv(h*p_pre*h'+q);%卡尔曼增益
            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);
            Mt_Est_Px(m,r)=x_est(1);%记录第m次仿真滤波估计数据
            Mt_Est_Py(m,r)=x_est(3);
        else
            if r==20
                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
%画图
figure(1);
plot(Rx,Ry,'k-',Zx,Zy,'g:',xn,yn,'r-.');
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);
%axis([1,N,0,1]);
title('x方向估计误差标准差');
subplot(2,1,2);
plot(delta_y);
%axis([1,N,0,1]);
title('y方向估计误差标准差');

%% 子函数
%% X_est,P_est返回第m次仿真第r个采样点的滤波结果
%% Xn_est,Pn_est记录每个模型对应的第m次仿真第r次仿真第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];
%所采用的第三个模型参数，模型一位非机动，模型二、三均为机动模型
%模型一
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;
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];%模型二
PHI{3,1}=PHI{2,1};%模型三
G{1,1}=[T/2,0;1,0;0,T/2;0,1];%模型一
G{1,1}(6,2)=0;
G{2,1}=[T^2/4,0;T/2,0;0,T^2/4;0,T/2;1,0;0,1];%模型二
G{3,1}=G{2,1};%模型三
Q{1,1}=zeros(2);%模型一
Q{2,1}=0.001*eye(2);%模型二
Q{3,1}=0.0114*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
for i=1:3
    mu(i,:)=P(i,:)*u(i)./c_mean;
end
%输入交互
for j=1:3
    X0{j,1}=zeros(6,1);
    P0{j,1}=zeros(6);
    for i=1:3
        X0{j,1}=X0{j,1}+Xn_est{i,1}*mu(i,j);
    end
    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
%模型条件滤波
a=zeros(1,3);
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}';
    %计算卡尔曼增益
    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
%模型概率更新
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;
    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});%观测相对于模型j的似然函数
end
c=sum(a.*c_mean);%归一化常数
u=a.*c_mean./c;%模型概率更新
%输出交互
Xn=zeros(6,1);
Pn=zeros(6);
for j=1:3
    Xn=Xn+Xn_est{j,1}.*u(j);
end
for j=1:3
    Pn=Pn+u(j).*(Pn_est{j,1}+(Xn_est{j,1}-Xn)*(Xn_est{j,1}-Xn)');
end
%返回滤波结果
X_est=Xn;
P_est=Pn;

目标的真实轨迹、观测轨迹、估计轨迹

滤波误差的均值曲线

滤波误差的标准差曲线

参考书籍：《卡尔曼滤波原理及仿真应用——MATLAB仿真》黄小平(著)