精华内容
下载资源
问答
  • 卡尔曼滤波的MATLAB实现。包括代码及详细说明,画出了各种不同的曲线。
  • 卡尔曼滤波的matlab实现方法,程序简单清晰,可靠性高.可以作为部分专业的课程设计使用。
  • 小波变换的3个MATLAB算法实现,另附有卡尔曼的MATLAB代码
  • 卡尔曼滤波Matlab实现

    万次阅读 多人点赞 2018-08-07 01:19:53
    卡尔曼滤波原理浅析 一、什么是卡尔曼滤波  滤波是从信号中提取有用信息的过程,比如从电信号中提取有用的频谱分量,从观测到的物体...1.1卡尔曼滤波的目标和使用条件  卡尔曼滤波是一种线性最小方差估计,目...

    卡尔曼滤波及Matlab实现

    一、什么是卡尔曼滤波

     滤波是从信号中提取有用信息的过程,比如从电信号中提取有用的频谱分量,从观测到的物体轨迹中提取位置信息,滤除图像信号中的噪声等。卡尔曼滤波是一种有效的滤波方法。如果已知一个系统的状态方程,又可以通过外部手段对系统进行观测,得到量测方程,就可以应用卡尔曼滤波估计系统的状态。

    1.1卡尔曼滤波的目标和使用条件

     卡尔曼滤波是一种线性最小方差估计,目的是使估计的均方误差最小,即:
    E{[XX^(Z)][XX^(Z)]T}E\{[X-\hat{X}(Z)][X-\hat{X}(Z)]^T\}

    最小。X^(Z)\hat{X}(Z)为用Z计算而得的X的最小方差估计值,且X^(Z)\hat{X}(Z)为量测向量Z的线性函数,即:
    X^=AZ+b \hat{X}=AZ+b
    X~=XX^\tilde{X}=X- \hat{X}为估计误差,又称残差。
    这里估计的XX是一个观测值,也就是实际情况中得到的数据,在组合导航中就是物体真实的运动参数。
    线性最小方差估计 X^\hat{X}具有以下性质:
    无偏性,即E{X^}=E{X}E\{\hat{X}\}=E\{X\}
    正交性,即E{[(XX^)ZT]}=0E\{[(X-\hat{X})Z^T]\}=0
    E{X~X~T}E\{\tilde{X} \cdot\tilde{X}^T\} 为估计均方误差阵。由最无偏性可得:
    E{[XX^]}=E{X~}=0E\{[X-\hat{X}]\}=E\{\tilde{X}\}=0
    估计的均方误差就是估计误差的方差,即:
    E{X~X~T}=E{[X~E(X~)][X~E(X~)]T}E\{\tilde{X} \cdot\tilde{X}^T\}=E\{[\tilde{X}-E(\tilde{X})] \cdot [\tilde{X}-E(\tilde{X})]^T\}
    由上式可以看出,最小方差估计不但使估值X ̂的均方误差最小,且这种最小均方误差就是X ̂的误差的方差。
     卡尔曼滤波适用于线性系统。要求系统的状态方程和量测方程线性。,即
    &X&
    系统的状态方程:Xk=Φ(k(k1))X(k1)+WkX_k=Φ_{(k⁄(k-1))} X_{(k-1)}+W_k
    系统的量测方程:Zk=HXk+VkZ_k=HX_k+V_k
      上面两个公式中,XkX_k是k时刻的系统状态,ZkZ_kkk时刻的量测值,H是量测系统的参数,WkW_kVkV_k分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的协方差(covariance)分别是QQRR(这里我们假设他们不随系统状态变化而变化)。
     计算机处理离散系统,但是现实世界中的系统大多是连续的。为了在计算机上计算,我们需要将连续系统转化为离散系统。在很多应用场景中,系统状态方程是微分方程(比如惯性导航系统),状态转移矩阵Φ(k(k1))Φ_{(k⁄(k-1))}可以通过求解微分方程得出。具体的解法在信号与系统教材中可以查到。总之,我们可以将微分方程变成离散的递推方程,以便编程。
    总结一下,卡尔曼滤波的适用条件为:
    1、状态方程和量测方程线性
    2、系统噪声和量测噪声为白噪声
    3、QQRR为定值,不随时间变化。

    1.2 卡尔曼滤波公式

    这里写图片描述
     卡尔曼滤波的流程如上图。
    ①式为状态一步预测方程,通过上个时刻的估计值预测出一个此时刻的一步预测值X ̂_(k⁄(k-1))。
    ②式是一步预测均方误差方程,P_(k∕(k−1))是X ̂_(k⁄(k-1))的方差。
    ③式计算滤波增益K_k。可以直观的看出量测噪声协方差R越大,K_k越小。K_k反映出一步预测值和量测值在估计值中的占比。
    ④式是状态估计方程,由两部分组成:一步预测值和新息过程乘以滤波增益。我们把Z_k−H_k (X ̂_(k∕[k−1]))称为新息过程(innovation),可以理解成Z_k为系统注入了新的信息。
    ⑤式计算估计均方误差,即P_k是状态估计值X ̂_k的方差。同时,根据卡尔曼滤波的定义,P_k也是评判卡尔曼滤波效果的标准。滤波趋于稳定之后,P_k 会稳定在一个固定的较小值附近。P_k越小,说明均方误差越小,卡尔曼滤波效果越好。

    在以上公式中还有一些需要注意的地方:
    1、对于有些系统,Φ_(k⁄(k-1)) 是时不变的,比如匀速直线运动系统。但是对于有些系统,比如惯性导航系统,Φ_(k⁄(k-1)) 时变,Φ_(k⁄(k-1)) 与系统中状态变量的输入量有关。
    2、如果Φ_(k⁄(k-1)) 时不变,P_k只与先验信息Q和R有关。也就是说,P_k完全由先验信息决定,与量测值无关。如果建模不准确,P_k可能不能准确反映X_k的真实情况,会导致滤波发散。
    3、Φ_(k⁄(k-1)) 时变的情况稍后分析。

    1.3 卡尔曼滤波和INS/GPS组合导航

    INS/GPS组合导航系统的公式太复杂,这里就不写了。INS/GPS组合导航系统的状态方程是惯导的误差方程。我们需要根据先验信息估计惯导的误差,解算出的经纬度等信息用来更新Φ_(k⁄(k-1)) ,这里Φ_(k⁄(k-1)) 是时变的。注意,我们需要根据先验信息确定一部分F矩阵的值,F矩阵的另一部分和姿态矩阵,速度、经纬度有关,姿态矩阵、速度和经纬度由惯导传感器量测值经解算、滤波后得出。F矩阵积分得到Φ_(k⁄(k-1)) 。如果惯导传感器的测量值误差太大,即实际的Q值偏离建模时使用的根据先验估计得到的Q值太多,会导致滤波发散。

    1.4 卡尔曼滤波的拓展

    1.1节中阐述了卡尔曼滤波的适用条件。研究者在标准卡尔曼滤波的基础上做了一些拓展,是卡尔曼滤波可以适用于更宽松的条件。如果系统非线性,可以使用扩展卡尔曼滤波或者无迹卡尔曼滤波。如果R和Q不满足白噪声要求,即R和Q时变,可以采用自适应卡尔曼滤波。

    二、Matlab程序示例

    2.1 匀速直线运动

    function main
    clc;clear;
    T=1;%雷达扫描周期
    N=100/T;%总的采样次数
    X=zeros(4,N);%目标真实位置、速度
    X(:,1)=[-100,2,200,20];%目标初始位置、速度
    S(:,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=eye(2);%观测噪声均值
    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];%观测矩阵
    Xn=zeros(4,0);
    for i=2:N
        S(:,i)=phi*S(:,i-1);%目标理论轨迹
        X(:,i)=phi*X(:,i-1)+sqrtm(Q)*randn(4,1);%目标真实轨迹
        Z(:,i)=H*X(:,i)+sqrtm(R)*randn(2,1);%对目标的观测
    end
    
    % Kalman 滤波
    Xkf=zeros(4,N);
    Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
    M(1,:)=Xkf(:,1);
    P0=100e-2*eye(4);%协方差阵初始化
    
    
    for i=2:N
        Xn=phi*Xkf(:,i-1);%预测
        M(i,:)=Xn;
        P1=phi*P0*phi'+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(S(1,:),S(3,:),'g','LineWidth',1);%真实轨迹
    % plot(X(1,:),X(3,:),'b','LineWidth',1);%观测轨迹
    % plot(Z(1,:),Z(2,:),'r','LineWidth',1);%观测轨迹
    % plot(Xkf(1,:),Xkf(3,:),'c','LineWidth',1);%卡尔曼滤波轨迹
    % plot(M(1,:),M(3,:),'k','LineWidth',1);%一步预测轨迹
    % legend('理论轨迹','实际运动轨迹','观测轨迹','滤波后轨迹','一步预测轨迹');
    % xlabel('横坐标 X/m');
    % ylabel('纵坐标 Y/m');
     
    figure
    hold on;box on;
    plot(Err_Observation);
    plot(Err_KalmanFilter);
    legend('滤波前误差','滤波后误差');
    xlabel('观测时间/s');
    ylabel('误差值');
    
    % 计算欧氏距离子函数
    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
    
    展开全文
  • 权威使用matlab实现卡尔曼滤波的书籍,包括理论和实践。Kalman Filtering - Theory and Practice Using MATLAB - Mohinder Grewal (Wiley, 2001, 0471392545)
  • 当涉及到卡尔曼滤波的时候我们先想一下下面的问题 为什么用卡尔曼滤波,即它可以解决什么类型的问题? 怎么用? 思考一下可能是这样: 为什么用卡尔曼滤波,即它可以解决什么类型的问题? 怎么用? ...

    1.序幕

    • 确定性的信号滤波可以直接用常用的滤波器就可以完成,如高通低通滤波器
    • 而随机信号的滤波(也可以称为信号估计)需要采用一种特殊的滤波器,如维纳滤波器(WienerFiltering)卡尔曼滤波器(KalmanFiltering),可以想象这与常规的滤波器完成的工作仍是类似的
    • 维纳滤波器处理的是平稳的随机信号,卡尔曼滤波器处理的是非平稳(或平稳)的随机信号
    • 且前提是线性系统,而非线性的则需要改进的方法(暂不讨论)。
    • 简单而言,卡尔曼滤波是在观测值与预测值之间取一个衡量,来作为一个最优的估计值

    2.基础需求

    如何缕清思路:当涉及到卡尔曼滤波的时候我们先想一下下面的问题

    1. 为什么用卡尔曼滤波,使用的前提是什么,它可以解决什么类型的问题?
      1. 卡尔曼滤波使用条件:1、线性系统; 2、系统中噪声(不确定性)服从高斯分布
      2. 卡尔曼滤波解决的问题:有不确定因素的动态系统,有根据的预测状态
    2. 怎么使用卡尔曼滤波器?

    如果仅需要使用滤波器,那么知道下面的公式便足够了,讲实际问题缕清关系,明确输入输出,转换为程序就可以处理信号了(具体问题如何建模可以参考第四部分的实例)

    part 1.系统状态和测量方程

    方程类型 方程
    系统的状态方程 sk+1=Φk+1,ksk+Γknk{s}_{k+1}=\Phi_{k+1,k}·{s}_{k}+\Gamma_k·n_k
    系统的测量方程 zk=Hksk+wkz_k=H_k·s_k+w_k

    sk+1{s}_{k+1}--------状态矢量
    Φk+1,k\Phi_{k+1,k}--------状态转移矩阵
    Γk\Gamma_k--------扰动矩阵
    nkn_k--------扰动矢量
    zkz_k--------观测矢量
    HkH_k--------测量矩阵
    wkw_k--------测量噪声矢量

    part 2.参数计算
    P0=0P_0=0可视为约定初值
    E(nknlT)=QkδklE(n_kn_l^T)=Q_k\delta_{kl}
    E(wkwlT)=RkδklE(w_kw_l^T)=R_k\delta_{kl}

    part 3.卡尔曼滤波过程

    步骤 公式
    ①一步预测 s^k/k1=Φk,k1s^k1/k1\hat{s}_{k/k-1}=\Phi_{k,k-1}·\hat{s}_{k-1/k-1}
    ②预测误差的方差 Pk/k1s~=Φk,k1Pk1/k1s~Φk,k1T+Γk1Qk1Γk1TP^{\widetilde{s}}_{k/k-1}=\Phi_{k,k-1}·P^{\widetilde{s}}_{k-1/k-1}·\Phi_{k,k-1}^{T}+\Gamma_{k-1}·Q_{k-1}·\Gamma_{k-1}^{T}
    ③卡尔曼增益 Kk=Pk/k1s~HkT(HkPk/k1s~HkT+Rk)1K_{k}=P^{\widetilde{s}}_{k/k-1}H_k^T(H_kP^{\widetilde{s}}_{k/k-1}H_k^T+R_k)^{-1}
    ④滤波 s^k/k=s^k/k1+Kk(zkHks^k/k1)\hat{s}_{k/k}=\hat{s}_{k/k-1}+K_k(z_k-H_k\hat{s}_{k/k-1})
    ⑤滤波误差的方差阵 Pk/ks~=(IKkHk)Pk/k1s~P^{\widetilde{s}}_{k/k}=(I-K_kH_k)·P_{k/k-1}^{\widetilde{s}}

    变量的含义
    Φk,k1\Phi_{k,k-1}--------状态转移矩阵
    s^k/k1\hat{s}_{k/k-1}--------当前预测值
    s^k1/k1\hat{s}_{k-1/k-1} --------上一时刻的预测值
    BB --------控制矩阵
    Pk/k1s~P^{\widetilde{s}}_{k/k-1}--------状态协方差的估计值
    QQ--------状态转移协方差矩阵
    RkR_k--------观测协方差
    KkK_{k}--------卡尔曼增益
    zkz_k--------观测值
    s^k/k\hat{s}_{k/k}--------状态最优估计值
    HkH_k--------观测矩阵

    注:关于各个变量的含义,更细节的了解参考CSDN.blogx

    3.深层思考

    1. 为什么是卡尔曼滤波
    2. 卡尔曼滤波的提出思路是什么
    3. 如何化抽象为具体

    4.举个例子

    1.举一个简单的例子

    关于最常见的温度的例子
    某个房间的温度,我们在理论上认为是恒定不变的,设为22°,实际上有“噪声”的加入,使得温度在改变,因此我们可以这样建立模型

    part 1.系统状态和测量方程

    方程类型 方程 建模
    系统的状态方程 sk+1=Φk+1,ksk+Γknk{s}_{k+1}=\Phi_{k+1,k}·{s}_{k}+\Gamma_k·n_k sk+1=1sk+1nk{s}_{k+1}=1·{s}_{k}+1·n_k
    系统的测量方程 zk=Hksk+wkz_k=H_k·s_k+w_k zk=1sk+wkz_k=1·s_k+w_k

    part 2.参数计算
    P0=0P_0=0
    QkQ_k可设置为0.10.1
    E(wkwlT)=RkδklE(w_kw_l^T)=R_k\delta_{kl}可以选择计算出来

    part 3.卡尔曼滤波过程

    步骤 公式
    ①一步预测 s^k/k1=Φk,k1s^k1/k1\hat{s}_{k/k-1}=\Phi_{k,k-1}·\hat{s}_{k-1/k-1}
    ②预测误差的方差 Pk/k1s~=Φk,k1Pk1/k1s~Φk,k1T+Γk1Qk1Γk1TP^{\widetilde{s}}_{k/k-1}=\Phi_{k,k-1}·P^{\widetilde{s}}_{k-1/k-1}·\Phi_{k,k-1}^{T}+\Gamma_{k-1}·Q_{k-1}·\Gamma_{k-1}^{T}
    ③卡尔曼增益 Kk=Pk/k1s~HkT(HkPk/k1s~HkT+Rk)1K_{k}=P^{\widetilde{s}}_{k/k-1}H_k^T(H_kP^{\widetilde{s}}_{k/k-1}H_k^T+R_k)^{-1}
    ④滤波 s^k/k=s^k/k1+Kk(zkHks^k/k1)\hat{s}_{k/k}=\hat{s}_{k/k-1}+K_k(z_k-H_k\hat{s}_{k/k-1})
    ⑤滤波误差的方差阵 Pk/ks~=(IKkHk)Pk/k1s~P^{\widetilde{s}}_{k/k}=(I-K_kH_k)·P_{k/k-1}^{\widetilde{s}}

    根据滤波公式编出滤波函数,这里定义函数kalmans.mkalmans.m
    为了体现出起迭代性,让函数每次接收预测值,观测值,以及其他的为常量和计算值
    每次输出的是最优估计和下次处理需要使用的协方差阵

    % function kalmans.m
    function [Sk,Pk] = kalmans(z,Sa,Pa,Hk,Q,R,Phi,Gamma)
    Sp = Phi*Sa;
    Pp = Phi*Pa*Phi.' + Gamma*Q*Gamma.';
    Kk = Pp*Hk.'*(Hk*Pp*Hk.' + R);
    Sk = Sp + Kk*(z - Hk*Sp);
    Pk = (eye(size(Kk*Hk)) - Kk*Hk)*Pp;
    end
    
    % kalmanexample.m
    clc;clear all;close all;
    t = 1:100;
    tm = 22 + 0.2*randn(1,length(t));
    for i = 1:length(tm)
        z = tm(i);
        if(i==1) Sa = 00;Pa = 0;
        else Sa = Sk;Pa = Pk;
        end
        Hk = 1;
        Q = 0.1;R = cov(tm(i));Phi = 1;Gamma = 1;
        [Sk,Pk] = kalmans(z,Sa,Pa,Hk,Q,R,Phi,Gamma);
        Ski(i) = Sk;
    end
    figure;hold on;grid on;
    plot(tm,'*');
    plot(Ski);
    title('kalmanfilering');xlabel('t/h');ylabel('T/°C');
    

    图像
    在这里插入图片描述
    可以看出,估计曲线很快收敛到实际的温度上,并且很稳定,这部分的R用的是由观测数据的协方差计算出的值,我们可以通过测试不同的参数值去看参数的影响
    (Q=0.1,R=0)(Q=0.1,R=0)
    在这里插入图片描述
    (Q=0.1,R=1)(Q=0.1,R=1)
    在这里插入图片描述
    由于R所代表的是观测值协方差,当R的取值越大的时候,可以预想到,在最优估计中,观测值所占的比重越大,估计值占的比重越小,所以最优估计值追随观测值的速度越快,反之亦是。不过需要注意的是R应该是已知条件,可由观测数据得到,而Q则视为未知的,其被用来表示状态转换矩阵与实际过程之间的误差。而我们无法直接观测到过程信号, 所以 Q 的取值是很难确定的。是系统观测模型本身带来的误差

    2.稍稍复杂的例子

    5.小结

    对于“算法研究者”而言,当问题分析建模结束后,整体的工作也就结束了,编程只需要按照逻辑,定义变量,嵌入运算,检验运行结果与预期对比
    作为初学者,只是对公式以及运用进行尝试性的理解,很多内涵没有理解到,同时也可能存在一些想错的地方,会逐步的理解修改

    参考

    1. http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ &
    2. 我所理解的卡尔曼滤波
    3. 卡尔曼滤波器(PPT)
    4. 卡尔曼参数及意义
    5. 卡尔曼滤波基础编程
    展开全文
  • 卡尔曼滤波的matlab简单实现,在三维空间,采用CV模型
  • 卡尔曼滤波实现简单,滤波效果好 ,下面分享一个基于卡尔曼滤波的matlab算法,数据全部为一维状态,本人弥补的详细备注,供爱好者研究学习。%%%%%%%%%%%%%%%%%%%功能说明:Kalman滤波用在一维温度数据测量系统中...

    卡尔曼滤波实现简单,滤波效果好 ,下面分享一个基于卡尔曼滤波的matlab算法,数据全部为一维状态,本人弥补的详细备注,供爱好者研究学习。%%%%%%%%%%%%%%%%%%

    %功能说明:Kalman滤波用在一维温度数据测量系统中

    function main

    %%%%%%

    N = 120;                                    %一共采样的点数,时间单位是分钟,可理解为实验进行了60分钟的测量

    CON = 25;                                %室内温度的理论值,在这个理论值的基础上受过程噪声会有波动

    %ones(a,b) 产生a行b列值为1的矩阵

    %zeros(a,b)产生a行b列值为0的矩阵

    Xexpect = CON*ones(1,N);     %期望的温度是恒定的25度,但实际温度不可能会这样的

    X = zeros(1,N);                         %房间各时刻真实温度值

    Xkf = zeros(1,N);                      %Kalman滤波处理的状态,也叫估计值

    Z = zeros(1,N);                         %温度计测量值

    P = zeros(1,N);

    %X(1)为数组的第一个元素

    X(1) = 25.1;     %假如初始值房间温度为25.1度

    P(1) = 0.01;     %初始值的协方差   (测量值 - 真实值)^2

    %产生0-1的随机数  模拟系统的随机数据

    Z = 24+rand(1,N)*10 - 5;

    Z(1) = 24.9;     %温度计测的第一个数据

    Xkf(1) = Z(1);  %初始测量值为24.9度,可以作为滤波器的初始估计状态噪声

    Q = 0;        %扰动误差方差(不存在扰动误差只有测量误差)

    R = 0.25;        %测量误差方差

    %sqrt(Q)*randn(1,N)为产生方差为0.01的随机信号

    %W为过程噪声

    %V为测量噪声

    W = sqrt(Q)*randn(1,N); %方差决定噪声的大小

    V = sqrt(R)*randn(1,N);%方差决定噪声的大小

    %系统矩阵

    %解释:因为该系统的数据都是一维的,故各变换矩阵都是1,原因自己找书理解

    F = 1;

    G = 1;

    H = 1;

    %eye产生m×n的单位矩阵 数值应该为1

    I = eye(1);  %本系统状态为一维

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %模拟房间温度和测量过程,并滤波

    for k = 2:N

    %第一步:随时间推移,房间真实温度波动变化

    %k时刻房间的真实温度,对于温度计来说,这个真实值是不知道的但是它的存在又是客观事实,读者要深刻领悟这个计算机模拟过程

    X(k) = F*X(k - 1)+G*W(k - 1);%实际值为理想值叠加上扰动噪声

    %第二步:随时间推移,获取实时数据

    %温度计对K时刻房间温度的测量,Kalman滤波是站在温度计角度进行的,

    %它不知道此刻的真实状态X(k),只能利用本次测量值Z(k)和上一次估计值Xkf(k)来做处理,其目标是最大限度地降低测量噪声R的影响

    %尽可能的逼近X(k),这也是Kalman滤波目的所在

    %修改本次函数

    %Z(k) = H*X(k)+V(k);                %测量值为实际值叠加上测量噪声

    %第三步:Kalman滤波

    %有了k时刻的观测Z(k)和k-1时刻的状态,那么就可以进行滤波了,

    %读者可以对照(3.36)到式(3.40),理解滤波过程

    X_pre = F*Xkf(k - 1);                        %状态预测          X_pre为上一次卡尔曼滤波值

    P_pre = F*P(k - 1)*F + Q;                %协方差预测

    %inv()为求一个方阵的逆矩阵。

    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);     %Kalman估计与真实值之间的偏差

    for k = 1:N

    Err_Messure(k) = abs(Z(k) - X(k));    %abs()为求解绝对值

    Err_Kalman(k) = abs(Xkf(k) - X(k));

    end

    t = 1:N;

    figure   %画图显示

    %依次输出理论值,叠加过程噪声(受波动影响)的真实值

    %温度计测量值,Kalman估计值

    plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');

    legend('期望值','真实值','观测值','Kalman滤波值');

    xlabel('采样时间/s');

    ylabel('温度值/C');

    %误差分析图

    figure  %画图显示

    plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k*');

    legend('测量偏差','Kalman滤波偏差');

    xlabel('采样时间/s');

    ylabel('温度偏差值/C');

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    展开全文
  • 目标跟踪基于卡尔曼滤波的matlab程序实现
  • 卡尔曼滤波应用及其matlab实现

    万次阅读 多人点赞 2018-02-11 20:32:18
    卡尔曼滤波在温度测量中应用 X(k)=A*X(k-1)+T*W(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...

    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仿真》黄小平(著)

    展开全文
  • 卡尔曼滤波 —— MATLAB实现

    千次阅读 2019-07-14 20:12:17
    简单来说,卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。 对于解决很大部分问题,他是最优,效率最高甚至是最有用。 他广泛应用已经超过30年,包括...
  • 卡尔曼滤波matlab实现

    万次阅读 2017-03-18 13:44:11
    在这一部分,我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面描述,会涉及一些基本概念知识,包括概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space...
  • 卡尔曼滤波-Matlab程序

    2018-12-26 00:17:30
    卡尔曼滤波程序Matlab实现。 Kalman滤波在测量方差已知情况下能够从一系列存在测量噪声数据中,估计动态系统状态. 由于, 它便于计算机编程实现, 并能够对现场采集数据进行实时更新和处理, Kalman滤波是...
  • 卡尔曼滤波/matlab仿真

    2021-03-31 09:48:17
    卡尔曼滤波的simulink仿真见(包括离散时间,连续时间和混合时间卡尔曼滤波器simulink仿真) https://download.csdn.net/download/weixin_44044161/16262954 运动模型和量测模型均为线性 %参数设置:初始位置为0,...
  • 卡尔曼滤波MATLAB代码实现

    千次阅读 2018-10-20 15:22:45
    所以楼主参考网上资料结合一个小例子整理出卡尔曼滤波的MATLAB代码实现。看懂这些代码我们需要对卡尔曼滤波算法有基本的理解。大致的理解就是卡尔曼滤波算法通过两个量对系统进行最优估计,而这两个量应该占多大的...
  • 卡尔曼滤波算法 的MATLAB实现 压缩包直接打开即可
  • 模拟退火算法简介下面以一个实例来讲解模拟退火算法求解TSP问题城市坐标第一步就是加载城市坐标,然后处理第二步计算各个城市之间距离,并创建距离矩阵,我采用各个城市之间距离计算公式为距离矩阵是一个对称...
  • 模拟退火算法就是在搜索过程中,不仅接受更优解,而且在满足一定概率情况下接受劣解。接受更优解使解不断收敛,接受劣解有助于跳出某个局部最优解。模拟退火算法Part 1 模拟退火算法自然界原型Point 1 退火...
  • 卡尔曼滤波的matlab仿真 前言 毕业设计时选择了超宽带定位设计的硬件电路设计,但苦于硬件难以取得大的进步,开始对算法的实现进行研究。通过阅读论文发现,很多人都对卡尔曼滤波进行各种各样的改进,以达到对数据...
  • 转载自:https://www.omegaxyz.com/2018/01/26/aco/(一)蚁群算法由来蚁群算法(ant colony optimization)最早是由Marco Dorigo等人在1991年提出,他们在研究新型算法过程中,发现蚁群在寻找食物时,通过分泌一种...
  •  首先,关于卡尔曼滤波理论定义、种类、算法实现过程步骤以及相关例子方面,博主有两个推荐。(1)网络资源博客方面我推荐下面这位博主,里面温度例子可以作为切入点去理解卡尔曼这种“最优化自回归数据处理...
  • 卡尔曼滤波预测轨迹,非常有用,从纳什均衡问题求解现状和粒子群算法发展出发,对纳什均衡问题进行分析,并且使用粒子群算法模拟纳什均衡博弈过程。最终使用Matlab软件实现,最后简单改进了一下算法初值和...

空空如也

空空如也

1 2 3 4 5 ... 13
收藏数 246
精华内容 98
关键字:

卡尔曼滤波的matlab实现

matlab 订阅