精华内容
下载资源
问答
  • 自适应卡尔曼滤波在变形监测数据处理的应用
  • 这是一个抗差自适应滤波算法,大家可以用来设计,请大家下载
  • 该算法利用阈值自动选择开窗窗口的长度调节自适应因子,以此调整扩展卡尔曼滤波法(EKF)与无迹卡尔曼滤波法(UKF)中的滤波增益,进而合理利用测量信息,由此分别形成AEKF与AUKF算法。将两种方法分别应用于全球导航...
  • 提出了一种新的自适应卡尔曼滤波算法。该算法假设系统过程噪声方差和量测噪声方差之间存在的函数关系已知,两种噪声方差随着时间变化且均未知。先令当前时刻的过程噪声方差等于前一时刻的过程噪声方差,通过变分贝叶斯...
  • 这是一份matlab实现的自适应卡尔曼滤波器的代码,自适应卡尔曼的用途很广,这是一个代码包。
  • 简单的IMM卡尔曼滤波,基础的跟踪算法,希望对大家有用
  • 自适应卡尔曼滤波实例,MATLAB

    热门讨论 2013-11-18 10:44:10
    自适应卡尔曼滤波在变形监测数据处理的应用
  • 卡尔曼滤波与自适应卡尔曼滤波matlab例程.zip
  • 自适应卡尔曼滤波
  • 针对Allan方差法确定光纤陀螺ARW(angle random walk)噪声系数的一些不足,如大量存储数据、非实时处理、计算量大、耗时长等,提出了基于自适应卡尔曼滤波的光纤陀螺ARW系数在线估计方法。在角度随机游走、零偏不...
  • 本资源使用的是MATLAB进行编程。使用卡尔曼滤波的方法进行室内温度的预测,室内温度设为恒温25度。
  • 在无迹卡尔曼滤波的基础上进行了改进,设计的自适应无迹卡尔曼滤波算法,适合有无迹卡尔曼滤波基础的人进行学习。
  • 自适应卡尔曼滤波在变形监测数据处理的应用
  • 卡尔曼滤波作为一种动态数据处理方法已广泛应用于变形监测数据处理中,而自适应卡尔曼滤波能更好地解决滤波发散的问题。本文利用已有的多次沉降监测数据,通过MATLAB编程实现,建立模型,模拟建(构)筑物沉降的变化规律,...
  • 自适应卡尔曼滤波也有很多文献有相关的介绍,其中用的比较多的有基于Sage-Husa算法实现的。这段时间刚好用到,顺便做了一个,参考了一些网上的程序如: https://zhuanlan.zhihu.com/p/100074040 基本卡尔曼以及...

    卡尔曼滤波在信号处理方面用的是比较多的,资料也是非常多的,这里就不写了。自适应卡尔曼滤波也有很多文献有相关的介绍,其中用的比较多的有基于Sage-Husa算法实现的。这段时间刚好用到,顺便做了一个,参考了一些网上的程序如:

    https://zhuanlan.zhihu.com/p/100074040

    基本卡尔曼以及状态和测量方程方面采用了原来程序的一些内容。Sage-Husa部分进行了改进,主程序如下:

    close all;
    clear all;
    N=1000;
    % inlitial Kalman Filter
    qu=1;
    F = [0.0673 0.1553; 1 0];
    G = [1 -0.575 ; 0 0];
    H = [1 0];
    a=randn(1,N)*sqrt(qu);
    Q0=diag([qu qu]); %process noise
    R0 = 1;   %measurement noise 
    P0=eye(2)*10000;
    y=zeros(1,N);
    y(1)=a(1);
    y(2)=a(2)-0.575*a(1);
    for k=3:N
        y(k)=0.0673*y(k-1)+0.1553*y(k-2)+a(k)-0.575*a(k-1);
    end
    y = y+sqrt(R0)*randn(1,N);  %add the measure noise
    % because the measurement equal the state variable add a noise in one
    % dimension, so initiate it with y(1)
    X0=[y(1);0];
    b=0.8; % parameter of Sage Husa
    [X,P]=Sage_HusaKF(F,G,H,Q0,R0,X0,y,P0,b);
    
    % display the result
    t=1:N;
    figure
    subplot(2,1,1);
    plot(t,y,'b',t,X(1,:),'--r');
    legend('measure','estimated');
    title('Measure value and the estimated value');
    subplot(2,1,2);
    plot(t,y-X(1,:));
    title('error');
    

     其中调用的函数Sage_HusaKF如下:

    % modified version of Sage-Husa adeptive KF
    function [X,P]=Sage_HusaKF(F,G,H,Q0,R,X0,Z,P,b)
        N=size(Z,2);    %lenght of the measure data in the column
        M=length(X0);
        X=zeros(M,N);
        X(:,1)=X0;
        e=zeros(size(Z));
        q = zeros(M,1);
        Q = Q0;
        for k=2:N
            Ptemp=P;
            X_est=F*X(:,k-1);                  %计算一步预测估计:X(k/k-1)
            P_pre=F*P*F'+G*Q*G';               %一步预测估计的均方误差P(k/k-1)
            e(:,k)=Z(:,k)-H*X_est;           %计算残差epsilon(k)
            K=P_pre*H'*pinv((H*P_pre*H')+R);    %k时刻的增益阵
            X(:,k)=X_est+K*e(:,k);           %k时刻的状态估计X(k)
            P = (eye(M)-K*H)*P_pre;%  均方误差矩阵P(k)
    % Sage-Husa adeptive KF,这里做了一些修改
            d = (1-b)/(1-b^(N));
            q = (1-d*b^(k-1))*q +d*b^k*(X(:,k)-F*X(:,k-1));
            Q = (1-d*b^(k-1))*Q +d*b^k*(K*e(:,k)*e(:,k)'*K'+Ptemp-F*P*F');
        end
    end

    下面是一个仿真结果:

    效果基本还可以。 

    在下面的mathworks网站的“File Exchange”板块也上传一个英文版的,两个版本内容基本一致。

    https://www.mathworks.com/matlabcentral/fileexchange/89431-adaptive-kalman-via-a-modified-sage-husa-algorithm

    关于q和Q的推导如下图:

    需要说明的是基本卡尔曼滤波性能与矩阵P的初值关系很小,改进后滤波性能与P的初值相关性很高,在某些情况下可以较大地改进滤波效果。

    另外,该改进方法只对该特定的模型有效,其它的模型,试验过几个,都无效 。

    展开全文
  • 卡尔曼滤波原理及应用MATLAB仿真 <br/>《卡尔曼滤波原理及应用:MATLAB仿真》编辑推荐:《卡尔曼滤波原理及应用:MATLAB仿真》是作者精心为广大读者朋友们编写而成的此书。《卡尔曼滤波原理及应用:MATLAB仿真》...
  • 基于卡尔曼滤波的目标跟踪技术,详细的代码和详细的目标模型的建立,希望能对大家提供帮助,与大家互相交流技术。
  • 采用BP神经网络对系统进行辨识,获得精确的系统状态方程,利用新息自适应估计卡尔曼滤波算法中的过程噪声和测量噪声协方差矩阵,提出基于新息的神经网络自适应卡尔曼滤波算法。Matlab仿真结果表明,与传统卡尔曼滤波...
  • 卡尔曼滤波matlab程序实现

    千次阅读 多人点赞 2019-08-22 11:12:16
    转自:https://blog.csdn.net/zhangquan2015/article/details/79264540 原 卡尔曼滤波应用及其matlab实现 ...

    转自:https://blog.csdn.net/zhangquan2015/article/details/79264540

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

    版权声明:本文为博主原创文章,遵循 CC 4.0 by-sa 版权协议,转载请附上原文出处链接和本声明。
    本文链接: https://blog.csdn.net/zhangquan2015/article/details/79264540

    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');
    %%%%%%%%%%%%%%%%%%%%%%%%%
    
         
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51

    这里写图片描述
    这里写图片描述

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

    %%%%%%%%%%%%%
    % 卡尔曼滤波用于自由落体运动目标跟踪问题
    %%%%%%%%%%%%%%
    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('速度误差均方值')
    %%%%%%%%%%%%%%%%%%%%%%%
    
         
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73

    这里写图片描述
    这里写图片描述

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

    这里写图片描述
    这里写图片描述
    这里写图片描述
    这里写图片描述
    这里写图片描述
    这里写图片描述

    %  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
    %%%%%%%%%%%%%%%%%%%%%%%%
    
         
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56

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

    帧间差法目标检测

    % 目标检测函数,这个函数主要完成将目标从背景中提取出来
    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91

    视频目标跟踪

    %  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.01eye(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 = -1radius: radius/20 : 1radius
    r = sqrt(radius2-c2);
    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=Ax(i-1,:)’ + Bu
    end
    kfinit=1;
    PP = A
    PA’ + Q
    K = PP
    H’inv(HPPH’+R)
    x(i,:) = (xp + K
    ([cc(i),cr(i)]’ - Hxp))’;
    x(i,:)
    [cc(i),cr(i)]
    P = (eye(4)-K
    H)*PP

    hold on
    for c = -1radius: radius/20 : 1radius
    r = sqrt(radius2-c2);
    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 ® 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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125

    扩展卡尔曼滤波

    对于非线性系统滤波问题,常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题,其中应用最广泛的方法是扩展卡尔曼滤波(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');
    
     
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58

    真实值与估计值对比:
    这里写图片描述
    滤波误差:
    这里写图片描述

    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57

    这里写图片描述
    这里写图片描述

    基于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®randn(1,N);%均值为0,方差为1的高斯噪声
    for t=2:N
    X(:,t)=F
    X(:,t-1)+Gsqrtm(Q)randn(2,1);%目标真实轨迹
    end
    for t=1:N
    Z(t)=hfun(X(:,t),Xstation)+w(t);%目标观测
    %对sqrtm®w(t)转化为角度sqrtm®w(t)/pi180可以看出噪声的大小
    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’+GQG’;%预测误差协方差
    dd=hfun(Xn,Xstation);%观测预测
    %求雅克比矩阵H
    D=Dist(Xn,Xstation);
    H=[-(Xn(3,1)-y0)/D,0,(Xn(1,1)-x0)/D,0];%即为所求一阶近似
    K=P1H’inv(HP1H’+R);%增益
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-KH)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/pi180,’-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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82

    跟踪轨迹
    这里写图片描述
    跟踪误差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=Fex+Gu;
    %观测预测
    Zn=[atan( Xn(2)/sqrt(Xn(1)2+Xn(3)2) ),atan(-1Xn(3)/Xn(1))]’;
    %协方差阵预测
    P=F
    P0F’+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=PH’/(HPH’+R);
    %状态更新
    ex=Xn+K
    (z-Zn);
    %协方差阵更新
    P0=(eye(9)-K*H)*P;

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126

    寻的导弹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®randn(1,N);
    for t=2:N
    X(:,t)=F
    X(:,t-1)+Gsqrtm(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=FXsigma;
    %第三步:利用第二步的结果计算均值和协方差
    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+GQG’;
    %第四步:根据预测值,再一次使用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:2L+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);

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120

    跟踪轨迹
    这里写图片描述
    误差曲线
    这里写图片描述

    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,2L)];%权值
    Wc=Wm;
    Wc(1)=Wc(1)+(1-alpha^2+beta);%权值
    c=sqrt©;
    %第一步,获取一组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=X2diag(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§’;%Cholesky分解
    Y = X(:,ones(1,numel(X)));
    Xset = [X Y+A Y-A];

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115

    运动轨迹
    这里写图片描述
    跟踪误差
    这里写图片描述

    交互多模型卡尔曼滤波

    在卡尔曼滤波算法中用到了状态转移方程和观测方程,被估计量随时间变化,它是一种动态估计。在目标跟踪中,不必知道目标的运动模型就能够实时的修正目标的状态参量(位置、速度等),具有良好的适应性。但是当目标实施机动时,仅采用基本的卡尔曼滤波算法往往得不到理想的结果。这时需要采用自适应算法。交互多模型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,T2/2,0;0,1,0,0,T,0;0,0,1,T,0,T2/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}=[T2/4,0;T/2,0;0,T2/4;0,T/2;1,0;0,1];%模型二
    G{3,1}=G{2,1};%模型三
    Q{1,1}=zeros(2);%模型一
    Q{2,1}=0.001eye(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(HP_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.5v{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;

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208
    • 209
    • 210
    • 211
    • 212
    • 213
    • 214
    • 215
    • 216
    • 217
    • 218
    • 219
    • 220
    • 221
    • 222
    • 223
    • 224
    • 225
    • 226

    目标的真实轨迹、观测轨迹、估计轨迹
    这里写图片描述
    滤波误差的均值曲线
    这里写图片描述
    滤波误差的标准差曲线
    这里写图片描述

                                    </div>
                <link href="https://csdnimg.cn/release/phoenix/mdeditor/markdown_views-e44c3c0e64.css" rel="stylesheet">
                    </div>
    </article>
    
    <div class="content" style="width: 712px;">
    	<a href="https://blog.csdn.net/qq514218063/article/details/17034813" target="_blank" title="卡尔曼滤波及其MATLAB程序">
    	<h4 class="text-truncate oneline" style="width: 552px;">
    			<em>卡尔曼滤波</em><em>及其</em>MATLAB程序		</h4>
    	<div class="info-box d-flex align-content-center">
    		<p class="date-and-readNum oneline">
    			<span class="date hover-show">11-30</span>
    			<span class="read-num hover-hide">
    				阅读数 
    				2万+</span>
    			</p>
    		</div>
    	</a>
    	<p class="content" style="width: 712px;">
    		<a href="https://blog.csdn.net/qq514218063/article/details/17034813" target="_blank" title="卡尔曼滤波及其MATLAB程序">
    			<span class="desc oneline">今天写了个卡尔曼滤波的小程序,希望对有需要的同学有点帮助。卡尔曼滤波是一个很常用的滤波算法,与维纳滤波相比有很多长处。这里我们把KalmanFilter简称为KF。KF的基本思想是:采用信号、噪声、状...</span>
    		</a>
    		<span class="blog_title_box oneline ">
    								<span class="type-show type-show-blog type-show-after">博文</span>
    										<a target="_blank" href="https://blog.csdn.net/qq514218063">来自:	<span class="blog_title"> 张文宇的博客</span></a>
    											</span>
    	</p>
    </div>
    </div>
    
    <div class="comment-edit-box d-flex">
    	<a id="commentsedit"></a>
    	<div class="user-img">
    		<a href="//me.csdn.net/weixin_43377151" target="_blank">
    			<img class="" src="https://avatar.csdn.net/7/E/3/3_weixin_43377151.jpg">
    		</a>
    	</div>
    	<form id="commentform">
    		<input type="hidden" id="comment_replyId">
    		<textarea class="comment-content" name="comment_content" id="comment_content" placeholder="想对作者说点什么"></textarea>
    		<div class="opt-box"> <!-- d-flex -->
    			<div id="ubbtools" class="add_code">
    				<a href="#insertcode" code="code" target="_self"><i class="icon iconfont icon-daima"></i></a>
    			</div>
    			<input type="hidden" id="comment_replyId" name="comment_replyId">
    			<input type="hidden" id="article_id" name="article_id" value="79264540">
    			<input type="hidden" id="comment_userId" name="comment_userId" value="">
    			<input type="hidden" id="commentId" name="commentId" value="">
    			<div style="display: none;" class="csdn-tracking-statistics tracking-click" data-report-click="{&quot;mod&quot;:&quot;popu_384&quot;,&quot;dest&quot;:&quot;&quot;}"><a href="#" target="_blank" class="comment_area_btn">发表评论</a></div>
    			<div class="dropdown" id="myDrap">
    				<a class="dropdown-face d-flex align-items-center" data-toggle="dropdown" role="button" aria-haspopup="true" aria-expanded="false">
    				<div class="txt-selected text-truncate">添加代码片</div>
    				<svg class="icon d-block" aria-hidden="true">
    					<use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-triangledown"></use>
    				</svg>
    				</a>
    				<ul class="dropdown-menu" id="commentCode" aria-labelledby="drop4">
    					<li><a data-code="html">HTML/XML</a></li>
    					<li><a data-code="objc">objective-c</a></li>
    					<li><a data-code="ruby">Ruby</a></li>
    					<li><a data-code="php">PHP</a></li>
    					<li><a data-code="csharp">C</a></li>
    					<li><a data-code="cpp">C++</a></li>
    					<li><a data-code="javascript">JavaScript</a></li>
    					<li><a data-code="python">Python</a></li>
    					<li><a data-code="java">Java</a></li>
    					<li><a data-code="css">CSS</a></li>
    					<li><a data-code="sql">SQL</a></li>
    					<li><a data-code="plain">其它</a></li>
    				</ul>
    			</div>  
    			<div class="right-box">
    				<span id="tip_comment" class="tip">还能输入<em>1000</em>个字符</span>
    				<input type="button" class="btn btn-sm btn-cancel d-none" value="取消回复">
    				<input type="submit" class="btn btn-sm btn-red btn-comment" value="发表评论">
    			</div>
    		</div>
    	</form>
    </div>
    
    	<div class="comment-list-container">
    	<a id="comments"></a>
    	<div class="comment-list-box" style="max-height: 129px; display: block;"><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="10282706" data-replyname="weixin_44007832">        <a target="_blank" href="https://me.csdn.net/weixin_44007832"><img src="https://avatar.csdn.net/2/E/1/3_weixin_44007832.jpg" alt="weixin_44007832" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/weixin_44007832"><span class="name ">狗头军师-丁:</span></a>              <span class="comment">预计偏差P的值要开根号吧?</span><span class="date" title="2019-07-30 15:43:41">(3周前</span><span class="floor-num">#9楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="10282706"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="9940349" data-replyname="m0_37676429">        <a target="_blank" href="https://me.csdn.net/m0_37676429"><img src="https://avatar.csdn.net/8/9/5/3_m0_37676429.jpg" alt="m0_37676429" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/m0_37676429"><span class="name ">leo秋实:</span></a>              <span class="comment">在么大神?收到可以帮忙画个图么?微信电话 15524066346</span><span class="date" title="2019-06-07 07:37:12">(2个月前</span><span class="floor-num">#8楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="9940349"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="9823017" data-replyname="weixin_41346819">        <a target="_blank" href="https://me.csdn.net/weixin_41346819"><img src="https://avatar.csdn.net/4/9/C/3_weixin_41346819.jpg" alt="weixin_41346819" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/weixin_41346819"><span class="name ">weixin_41346819:</span></a>              <span class="comment">厉害</span><span class="date" title="2019-05-21 16:18:23">(3个月前</span><span class="floor-num">#7楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="9823017"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="9615409" data-replyname="weixin_37428348">        <a target="_blank" href="https://me.csdn.net/weixin_37428348"><img src="https://avatar.csdn.net/C/4/9/3_weixin_37428348.jpg" alt="weixin_37428348" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/weixin_37428348"><span class="name ">weixin_37428348:</span></a>              <span class="comment">膜拜</span><span class="date" title="2019-04-23 17:49:14">(3个月前</span><span class="floor-num">#6楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="9615409"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="8749044" data-replyname="zc2016jiayin">        <a target="_blank" href="https://me.csdn.net/zc2016jiayin"><img src="https://avatar.csdn.net/B/3/1/3_zc2016jiayin.jpg" alt="zc2016jiayin" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/zc2016jiayin"><span class="name ">zc2016jiayin:</span></a>              <span class="comment">大佬</span><span class="date" title="2018-11-22 19:30:56">(8个月前</span><span class="floor-num">#5楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="8749044"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="8455850" data-replyname="qq_41536517">        <a target="_blank" href="https://me.csdn.net/qq_41536517"><img src="https://avatar.csdn.net/5/3/7/3_qq_41536517.jpg" alt="qq_41536517" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/qq_41536517"><span class="name ">qq_41536517:</span></a>              <span class="comment">你好 请问能吧文中UKF有关的函数发给我吗 谢谢   QQ邮箱842755987@qq.com</span><span class="date" title="2018-09-15 17:00:58">(11个月前</span><span class="floor-num">#4楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="8455850"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="8455840" data-replyname="qq_41536517">        <a target="_blank" href="https://me.csdn.net/qq_41536517"><img src="https://avatar.csdn.net/5/3/7/3_qq_41536517.jpg" alt="qq_41536517" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/qq_41536517"><span class="name ">qq_41536517:</span></a>              <span class="comment">你好 请问能把文中的UKF函数发给我吗?谢谢  QQ邮箱842755987@qq.com。万分感谢</span><span class="date" title="2018-09-15 16:59:58">(11个月前</span><span class="floor-num">#3楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="8455840"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="8253312" data-replyname="qq_28137933">        <a target="_blank" href="https://me.csdn.net/qq_28137933"><img src="https://avatar.csdn.net/E/E/1/3_qq_28137933.jpg" alt="qq_28137933" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/qq_28137933"><span class="name ">qq_28137933:</span></a>              <span class="comment">你好在吗能帮忙做仿真吗,价格可商量,QQ 1170639733</span><span class="date" title="2018-07-25 15:42:19">(1年前</span><span class="floor-num">#2楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="8253312"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul><ul class="comment-list"><li class="comment-line-box d-flex" data-commentid="7982140" data-replyname="qq_42308876">        <a target="_blank" href="https://me.csdn.net/qq_42308876"><img src="https://avatar.csdn.net/E/C/6/3_qq_42308876.jpg" alt="qq_42308876" class="avatar"></a>          <div class="right-box ">            <div class="info-box">              <a target="_blank" href="https://me.csdn.net/qq_42308876"><span class="name ">qq_42308876:</span></a>              <span class="comment">你好 在吗 能留个联系方式吗 有偿服务 非常感谢 或者加我的QQ 178508014</span><span class="date" title="2018-05-25 13:31:14">(1年前</span><span class="floor-num">#1楼)</span><span class="opt-box"><a class="btn btn-link-blue btn-report" data-type="report">举报</a><a class="btn btn-link-blue btn-reply" data-type="reply">回复</a></span></div><div class="comment-like " data-commentid="7982140"><svg class="icon "><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-thumbsup"></use></svg><span></span></div></div></li></ul></div>
    	<div id="commentPage" class="pagination-box d-none" style="display: block;"><div id="Paging_09241115303493994" class="ui-paging-container"><ul><li class="js-page-first js-page-action ui-pager ui-pager-disabled"></li><li class="js-page-prev js-page-action ui-pager ui-pager-disabled">上一页</li><li data-page="1" class="ui-pager focus">1</li><li class="js-page-next js-page-action ui-pager ui-pager-disabled">下一页</li><li class="js-page-last js-page-action ui-pager ui-pager-disabled"></li></ul></div></div>
    	<div class="opt-box text-center">
    		<div class="btn btn-sm btn-link-blue" id="btnMoreComment"><span>查看 9 条热评</span><svg class="icon open" aria-hidden="true"><use xmlns:xlink="http://www.w3.org/1999/xlink" xlink:href="#csdnc-chevrondown"></use></svg></div>
    	</div>
    </div>
    

    卡尔曼滤波简介说明及其算法MATLAB实现代码

    04-18 阅读数 1万+

    卡尔曼滤波算法,很经典而且易懂,值得看。 博文 来自: qqh19910525的博客

    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_59" data-pid="59" data-report-view="{&quot;mod&quot;:&quot;kp_popu_59-78&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_59-78&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
    (function() {
        var s = "_" + Math.random().toString(36).slice(2);
        document.write('<div style="" id="' + s + '"></div>');
        (window.slotbydup = window.slotbydup || []).push({
            id: "u3491668",
            container:  s
        });
    })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_36750643/10239768&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;5&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_36750643/10239768&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;5&quot;}">
    	<a href="https://download.csdn.net/download/qq_36750643/10239768" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>的<em>MATLAB实现</em>					</h4>
    				<span class="data float-right">02-04</span>
    			</div>
    			<div class="desc oneline">
    					卡尔曼滤波的MATLAB实现。包括代码及详细说明,画出了各种不同的曲线。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/weixin_44044161/11050393&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;6&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/weixin_44044161/11050393&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;6&quot;}">
    	<a href="https://download.csdn.net/download/weixin_44044161/11050393" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>器matlab					</h4>
    				<span class="data float-right">03-22</span>
    			</div>
    			<div class="desc oneline">
    					关于kf的matlab程序,仿真实列为目标耿总问题,调用kf函数,实现滤波平滑				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_35632833/10876623&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;7&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_35632833/10876623&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;7&quot;}">
    	<a href="https://download.csdn.net/download/qq_35632833/10876623" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>-Matlab程序					</h4>
    				<span class="data float-right">12-26</span>
    			</div>
    			<div class="desc oneline">
    					卡尔曼滤波程序Matlab实现。 Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/linjieli_uestc/10770753&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;8&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/linjieli_uestc/10770753&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;8&quot;}">
    	<a href="https://download.csdn.net/download/linjieli_uestc/10770753" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em> Matlab程序					</h4>
    				<span class="data float-right">11-07</span>
    			</div>
    			<div class="desc oneline">
    					代码为书籍《卡尔曼滤波原理及应用》配套的代码,可用于卡尔曼滤波的学习				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/90769550&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;1&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/90769550&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;1&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/90769550&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:0,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/90769550&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:0,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/90769550" target="_blank">              		<h4 class="text-truncate oneline" style="width: 622px;">CMakeLists编写 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">6-17</span>                    </p>                  </div>                </a>            	</div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52137892&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;2&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52137892&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;2&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52137892&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:1,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52137892&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:1,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/52137892" target="_blank">              		<h4 class="text-truncate oneline" style="width: 622px;">基于Qt的QQ局域网聊天 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">5-29</span>                    </p>                  </div>                </a>            	</div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_60" data-pid="60" data-report-view="{&quot;mod&quot;:&quot;kp_popu_60-43&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_60-43&quot;,&quot;keyword&quot;:&quot;&quot;}"><div class="mediav_ad"><newsfeed class="newsfeed QIHOO__WEB__SO__1566442796312_460" id="QIHOO__WEB__SO__1566442796312_460" style="display:block;margin:0;padding:0;border:none;width:900px;height:84px;overflow-y:hidden;overflow-x:hidden;position:relative;text-align:left;"><info-div id="QIHOO__WEB__SO__1566442796312_460-info" style="zoom:1"><info-div class="QIHOO__WEB__SO__1566442796312_460 singleImage clk" data-href="https://s3.nzbdw.com/s?type=2&amp;r=20&amp;mv_ref=blog.csdn.net&amp;enup=CAABPZZFTQgAAk1Flj0A&amp;mvid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;bid=138b26aa9a90a369&amp;price=AAAAAF1eBSoAAAAAAAfavl/y6Ve50+eWfKL4qg==&amp;finfo=DAABCAABAAAAcQgAAgAAADcEAAM/YEDmNtDrOQAIAAIAAAADCgADTvv3UV+5cI8IAAQAAAA5BgAGLbcGAAoAAAgADgAAAB0KAA8AAAAAABE+EAA&amp;ugi=FcTgigEVyNtrTBUCFc4EFdoEFQAAFeGl/PUIFoQIFcgBFoD64KT8qsgFHBaNy7+FvZqH/8wBFQAAAA&amp;uai=FeTGlAIlAhUEFoCo1s20jvf7nQEV8ggllOq16A0lABUaFAAcFuPHo72Kx6qTzgEVAAAA&amp;ubi=FcidXBXKt+wCFay07hgV+Oz4WxUEFRwW/On9uhcWgKjrtL7U+/udATQCFrDgkIAIJQYVlImR0QwVwgUVADac87WxjaeY2xEA&amp;clickid=0&amp;cpx=__OFFSET_X__&amp;cpy=__OFFSET_Y__&amp;cs=__EVENT_TIME_START__&amp;ce=__EVENT_TIME_END__&amp;csign2=PmQ_DH6pDm2=&amp;url=http%3A%2F%2Fvip.qooo9.cn" data-pv="https://s3.nzbdw.com/s?type=1&amp;r=20&amp;tid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;finfo=DAABCAABAAAAcQgAAgAAADcEAAM/YEDmNtDrOQAIAAIAAAADCgADTvv3UV+5cI8IAAQAAAA5BgAGLbcGAAoAAAgADgAAAB0KAA8AAAAAABE+EAA&amp;mv_ref=blog.csdn.net&amp;enup=CAABPZZFTQgAAk1Flj0A&amp;mvid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;bid=138b26aa9a90a369&amp;ugi=FcTgigEVyNtrTBUCFc4EFdoEFQAAFeGl/PUIFoQIFcgBFoD64KT8qsgFHBaNy7+FvZqH/8wBFQAAAA&amp;uai=FeTGlAIlAhUEFoCo1s20jvf7nQEV8ggllOq16A0lABUaFAAcFuPHo72Kx6qTzgEVAAAA&amp;ubi=FcidXBXKt+wCFay07hgV+Oz4WxUEFRwW/On9uhcWgKjrtL7U+/udATQCFrDgkIAIJQYVlImR0QwVwgUVADac87WxjaeY2xEA&amp;ds=1&amp;price=AAAAAF1eBSoAAAAAAAfavl/y6Ve50+eWfKL4qg==,https://max-l.mediav.com/rtb?type=2&amp;ver=1&amp;v=CGQSEDEzOGIyNmFhOWE5MGEzNjkYsqOKASCisEUoAWIXNzQyMDc0MDQyMTMwNTEyMjQwMjAwMTmIAQA&amp;k=dVpuYQAAAAA=&amp;w=AAAAAF1eBSoAAAAAAAfa_AYIGcMC0una83tE2w&amp;i=5IvvYcy7TDZn&amp;exp=BQBECgBEAQJEBAJEEABDIgBD&amp;z=1" data-clk="https://max-l.mediav.com/rtb?type=3&amp;ver=1&amp;v=CGQSEDEzOGIyNmFhOWE5MGEzNjkYsqOKASCisEUoAWIXNzQyMDc0MDQyMTMwNTEyMjQwMjAwMTlwAA&amp;k=/nutgAAAAAA=&amp;i=5IvvYcy7TDZn&amp;exp=BQBECgBEAQJEBAJEEABDIgBD&amp;x=__OFFSET_X__&amp;y=__OFFSET_Y__&amp;st=__EVENT_TIME_START__&amp;et=__EVENT_TIME_END__&amp;adw=__ADSPACE_W__&amp;adh=__ADSPACE_H__&amp;tc=&amp;turl=">
    <info-div class="wrap">
        <info-div class="singleImage-img singleImage-img-left">
            <info-div class="img" style="background-image:url(https://s3m.nzwgs.com/galileo/755556-ffee2684cc43e378e978f83373b7b3d8.jpg)"><info-div class="ads-tag"></info-div></info-div>
        </info-div>
        <info-div class="singleImage-body singleImage-body-left">
            <info-div class="singleImage-title">老中医说:饭后用一物,变易瘦体质,想瘦多少就瘦多少</info-div>
            <info-div class="singleImage-desc">江汇 · 猎媒</info-div>
        </info-div>
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/yueroo/11150561&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;9&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/yueroo/11150561&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;9&quot;}">
    	<a href="https://download.csdn.net/download/yueroo/11150561" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em><em>MATLAB实现</em>					</h4>
    				<span class="data float-right">04-29</span>
    			</div>
    			<div class="desc oneline">
    					详细介绍了卡尔曼滤波原理,对卡尔曼滤波进行了MATLAB仿真设计				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/83183540&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;3&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/83183540&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;3&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/83183540&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:2,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/83183540&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:2,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/83183540" target="_blank">              		<h4 class="text-truncate oneline" style="width: 631px;">正则表达式验证合法电话号码 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">4-1</span>                    </p>                  </div>                </a>            	</div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52138303&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;4&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52138303&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;4&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52138303&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:3,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/52138303&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:3,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/52138303" target="_blank">              		<h4 class="text-truncate oneline" style="width: 622px;">Qt双击表格获取表格内容 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">6-13</span>                    </p>                  </div>                </a>            	</div>
    
    		<div class="recommend-item-box blog-expert-recommend-box" style="display: block;">
    		<div class="d-flex">
    			<div class="blog-expert-recommend">
    				<div class="blog-expert">
    					<div class="blog-expert-flexbox" data-report-view="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><div class="blog-expert-item"><div class="blog-expert-info-box"><div class="blog-expert-img-box" data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/qq514218063" target="_blank"><img src="https://avatar.csdn.net/4/F/B/3_qq514218063.jpg" alt="babybabytellmewhy" title="babybabytellmewhy"></a><span data-report-click="{&quot;mod&quot;:&quot;popu_710&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><span class="blog-expert-button-follow btn-red-follow" data-name="qq514218063" data-nick="babybabytellmewhy">关注</span></span></div><div class="info"><span data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/qq514218063" target="_blank"><h5 class="oneline" title="babybabytellmewhy">babybabytellmewhy</h5></a></span>  <p></p><p class="article-num" title="6篇文章"> 6篇文章</p><p class="article-num" title="排名:千里之外"> 排名:千里之外</p><p></p></div></div></div><div class="blog-expert-item"><div class="blog-expert-info-box"><div class="blog-expert-img-box" data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/OUYANG_LINUX007" target="_blank"><img src="https://avatar.csdn.net/B/4/3/3_ouyang_linux007.jpg" alt="OUYANG_LINUX007" title="OUYANG_LINUX007"></a><span data-report-click="{&quot;mod&quot;:&quot;popu_710&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><span class="blog-expert-button-follow btn-red-follow" data-name="OUYANG_LINUX007" data-nick="OUYANG_LINUX007">关注</span></span></div><div class="info"><span data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/OUYANG_LINUX007" target="_blank"><h5 class="oneline" title="OUYANG_LINUX007">OUYANG_LINUX007</h5></a></span>  <p></p><p class="article-num" title="66篇文章"> 66篇文章</p><p class="article-num" title="排名:千里之外"> 排名:千里之外</p><p></p></div></div></div><div class="blog-expert-item"><div class="blog-expert-info-box"><div class="blog-expert-img-box" data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/qqh19910525" target="_blank"><img src="https://avatar.csdn.net/6/D/C/3_qqh19910525.jpg" alt="如梦如幻2015" title="如梦如幻2015"></a><span data-report-click="{&quot;mod&quot;:&quot;popu_710&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><span class="blog-expert-button-follow btn-red-follow" data-name="qqh19910525" data-nick="如梦如幻2015">关注</span></span></div><div class="info"><span data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/qqh19910525" target="_blank"><h5 class="oneline" title="如梦如幻2015">如梦如幻2015</h5></a></span>  <p></p><p class="article-num" title="125篇文章"> 125篇文章</p><p class="article-num" title="排名:千里之外"> 排名:千里之外</p><p></p></div></div></div><div class="blog-expert-item"><div class="blog-expert-info-box"><div class="blog-expert-img-box" data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/worldbit" target="_blank"><img src="https://avatar.csdn.net/9/C/D/3_worldbit.jpg" alt="worldbit" title="worldbit"></a><span data-report-click="{&quot;mod&quot;:&quot;popu_710&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><span class="blog-expert-button-follow btn-red-follow" data-name="worldbit" data-nick="worldbit">关注</span></span></div><div class="info"><span data-report-click="{&quot;mod&quot;:&quot;popu_709&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/79264540&quot;}"><a href="https://blog.csdn.net/worldbit" target="_blank"><h5 class="oneline" title="worldbit">worldbit</h5></a></span>  <p></p><p class="article-num" title="8篇文章"> 8篇文章</p><p class="article-num" title="排名:千里之外"> 排名:千里之外</p><p></p></div></div></div></div>
    				</div>
    			</div>
    		</div>
    	</div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/76379164&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;5&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/76379164&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;5&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/76379164&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:4,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/76379164&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:4,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/76379164" target="_blank">              		<h4 class="text-truncate oneline" style="width: 622px;">OpenCV学习之图像尺寸变换 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">3-18</span>                    </p>                  </div>                </a>            	</div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/81347392&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;6&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/81347392&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;6&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/81347392&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:5,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/81347392&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:5,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/81347392" target="_blank">              		<h4 class="text-truncate oneline" style="width: 622px;">C++网络通信实现 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">8-12</span>                    </p>                  </div>                </a>            	</div>
    

    OpenCV学习之金字塔及其应用 - Joey's Blog - CSDN博客

    5-20

    第6章 最简单的界面化程序——对话框 - Joey's Blog - CSDN博客

    7-10

    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_61" data-pid="61" data-report-view="{&quot;mod&quot;:&quot;kp_popu_61-622&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_61-622&quot;,&quot;keyword&quot;:&quot;&quot;}"><div id="_irqkblbi1a8pu6kcka0b2o6r" style="width: 100%;"><div id="axytgfh" style="padding-right:0px;"><iframe width="852" frameborder="0" height="66" scrolling="no" src="https://pos.baidu.com/s?hei=66&amp;wid=852&amp;di=u3600846&amp;ltu=https%3A%2F%2Fblog.csdn.net%2Fzhangquan2015%2Farticle%2Fdetails%2F79264540&amp;psi=1f283c47c9792b12ed499c54965f226d&amp;cja=false&amp;pss=1858x45709&amp;ti=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E5%BA%94%E7%94%A8%E5%8F%8A%E5%85%B6matlab%E5%AE%9E%E7%8E%B0&amp;pis=-1x-1&amp;ari=2&amp;dis=0&amp;prot=2&amp;dtm=HTML_POST&amp;ant=0&amp;chi=1&amp;ps=43749x580&amp;cdo=-1&amp;psr=1920x1080&amp;cmi=6&amp;drs=1&amp;exps=111000,119009,110011&amp;par=1920x1040&amp;tpr=1566442796618&amp;col=zh-CN&amp;dc=3&amp;cce=true&amp;pcs=1858x920&amp;ccd=24&amp;cec=UTF-8&amp;cpl=4&amp;cfv=0&amp;dri=0&amp;ltr=https%3A%2F%2Fblog.csdn.net%2FAsc11_%2Farticle%2Fdetails%2F40714239&amp;tcn=1566442797&amp;tlm=1566442796&amp;dai=5"></iframe><div style="display:none;padding-right:0px;"></div></div></div><script type="text/javascript" src="//rabc1.iteye.com/common/web/production/79m9.js?f=aszggcwz"></script></div></div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/91537827&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;9&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/91537827&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;9&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/91537827&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:8,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/91537827&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:8,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/91537827" target="_blank">              		<h4 class="text-truncate oneline" style="width: 631px;">CMake交叉编译 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">7-1</span>                    </p>                  </div>                </a>            	</div><div class="recommend-item-box baiduSearch recommend-box-ident" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/80160864&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;10&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/80160864&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:&quot;10&quot;}" data-track-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/80160864&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:9,&quot;extend1&quot;:&quot;_&quot;}" data-track-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/zhangquan2015/article/details/80160864&quot;,&quot;strategy&quot;:&quot;searchFromBaidu1&quot;,&quot;index&quot;:9,&quot;extend1&quot;:&quot;_&quot;}" data-flg="true">                <a href="https://blog.csdn.net/zhangquan2015/article/details/80160864" target="_blank">              		<h4 class="text-truncate oneline" style="width: 631px;">C语言实现图像的读写旋转与缩放 - Joey's Blog - CSDN博客</h4>                  <div class="info-box d-flex align-content-center">                    <p>                      <span class="date">8-7</span>                    </p>                  </div>                </a>            	</div>
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_27432295/10268791&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;15&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_27432295/10268791&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;15&quot;}">
    	<a href="https://download.csdn.net/download/qq_27432295/10268791" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					扩展<em>卡尔曼滤波</em>程序示例(matlab)					</h4>
    				<span class="data float-right">03-04</span>
    			</div>
    			<div class="desc oneline">
    					matlab写的一个扩展卡尔曼滤波程序,状态方程为线性,观测方程非线性,最后输出图片以便观察是否收敛,分享给大家参考。还有一个C++版本的。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/yuchuan3912/1503931&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;16&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/yuchuan3912/1503931&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;16&quot;}">
    	<a href="https://download.csdn.net/download/yuchuan3912/1503931" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>matlab工具箱					</h4>
    				<span class="data float-right">07-20</span>
    			</div>
    			<div class="desc oneline">
    					一个国外网站上的程序包,里面有很成熟的算法,包括简单的例子介绍,相信对用卡尔曼滤波技术的朋友会有很大帮助				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_16006651/10180710&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;17&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_16006651/10180710&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;17&quot;}">
    	<a href="https://download.csdn.net/download/qq_16006651/10180710" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>Matlab					</h4>
    				<span class="data float-right">12-29</span>
    			</div>
    			<div class="desc oneline">
    					在Matlab里面,描述的卡尔曼滤波。清楚的描述了卡尔曼滤波的表达试				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_62" data-pid="62" data-report-view="{&quot;mod&quot;:&quot;kp_popu_62-623&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_62-623&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
    (function() {
        var s = "_" + Math.random().toString(36).slice(2);
        document.write('<div style="" id="' + s + '"></div>');
        (window.slotbydup = window.slotbydup || []).push({
            id: "u3600849",
            container:  s
        });
    })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/renzhu2007/3111646&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;23&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/renzhu2007/3111646&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;23&quot;}">
    	<a href="https://download.csdn.net/download/renzhu2007/3111646" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					姿态确定的扩展<em>卡尔曼滤波</em>Matlab					</h4>
    				<span class="data float-right">03-21</span>
    			</div>
    			<div class="desc oneline">
    					在姿态确定的四元数方程基础上,采用扩展卡尔曼滤波				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_63" data-pid="63" data-report-view="{&quot;mod&quot;:&quot;kp_popu_63-1405&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_63-1405&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
        (function() {
            var s = "_" + Math.random().toString(36).slice(2);
            document.write('<div style="" id="' + s + '"></div>');
            (window.slotbydup = window.slotbydup || []).push({
                id: "u4221910",
                container: s
            });
        })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_38490273/10296005&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;24&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/qq_38490273/10296005&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;24&quot;}">
    	<a href="https://download.csdn.net/download/qq_38490273/10296005" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>MATLAB代码					</h4>
    				<span class="data float-right">03-19</span>
    			</div>
    			<div class="desc oneline">
    					国外学者的开源卡尔曼滤波MATLAB代码,备注详细,可以用于目标跟踪基础学习				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/master03/10337780&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;25&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/master03/10337780&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;25&quot;}">
    	<a href="https://download.csdn.net/download/master03/10337780" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					经典<em>卡尔曼滤波</em> 目标跟踪 matlab 程序注释详细 新手入门					</h4>
    				<span class="data float-right">04-10</span>
    			</div>
    			<div class="desc oneline">
    					经典卡尔曼滤波 目标跟踪 程序注释详细 新手入门 matlab 程序无bug 完美运行 重要的事情说三遍!包教包会 包教包会 包教包会!				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/linhaiyan927/1791062&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;26&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/linhaiyan927/1791062&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;26&quot;}">
    	<a href="https://download.csdn.net/download/linhaiyan927/1791062" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>器MATLAB编程实现					</h4>
    				<span class="data float-right">11-03</span>
    			</div>
    			<div class="desc oneline">
    					矢量卡尔曼滤波器的MATLAB编程实现 利用卡尔曼滤波器实现去噪功能				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_64" data-pid="64" data-report-view="{&quot;mod&quot;:&quot;kp_popu_64-1379&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_64-1379&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
        (function() {
            var s = "_" + Math.random().toString(36).slice(2);
            document.write('<div style="" id="' + s + '"></div>');
            (window.slotbydup = window.slotbydup || []).push({
                id: "u4221811",
                container: s
            });
        })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/u011254129/10243082&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;32&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/u011254129/10243082&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;32&quot;}">
    	<a href="https://download.csdn.net/download/u011254129/10243082" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>工具箱					</h4>
    				<span class="data float-right">02-06</span>
    			</div>
    			<div class="desc oneline">
    					matlab的卡尔曼工具箱,利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/u014731502/7197251&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;33&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/u014731502/7197251&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;33&quot;}">
    	<a href="https://download.csdn.net/download/u014731502/7197251" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>与<em>matlab实现</em>					</h4>
    				<span class="data float-right">04-15</span>
    			</div>
    			<div class="desc oneline">
    					卡尔曼滤波入门,一个典型例子,并有matlab代码,适合新手				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_65" data-pid="65" data-report-view="{&quot;mod&quot;:&quot;kp_popu_65-1378&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_65-1378&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
        (function() {
            var s = "_" + Math.random().toString(36).slice(2);
            document.write('<div style="" id="' + s + '"></div>');
            (window.slotbydup = window.slotbydup || []).push({
                id: "u4221803",
                container: s
            });
        })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/hobbyjobs/9921989&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;34&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/hobbyjobs/9921989&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;34&quot;}">
    	<a href="https://download.csdn.net/download/hobbyjobs/9921989" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>原理及<em>应用</em> matlab仿真					</h4>
    				<span class="data float-right">08-05</span>
    			</div>
    			<div class="desc oneline">
    					卡尔曼滤波原理及应用 matlab仿真 黄小平,完整pdf外加matlab源码,很多地方都只提供部分内容				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    
    
    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/ysudykx/10041138&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;35&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/ysudykx/10041138&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;35&quot;}">
    	<a href="https://download.csdn.net/download/ysudykx/10041138" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					<em>卡尔曼滤波</em>算法MATLAB仿真					</h4>
    				<span class="data float-right">10-26</span>
    			</div>
    			<div class="desc oneline">
    					离散卡尔曼滤波算法MATLAB仿真,带有详细注释,有联系方式可以交流。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_66" data-pid="66" data-report-view="{&quot;mod&quot;:&quot;kp_popu_66-87&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_66-87&quot;,&quot;keyword&quot;:&quot;&quot;}"><div class="mediav_ad"><newsfeed class="newsfeed QIHOO__WEB__SO__1566442796700_717" id="QIHOO__WEB__SO__1566442796700_717" style="display:block;margin:0;padding:0;border:none;width:852px;height:60px;overflow-y:hidden;overflow-x:hidden;position:relative;text-align:left;"><info-div id="QIHOO__WEB__SO__1566442796700_717-info" style="zoom:1"><info-div class="QIHOO__WEB__SO__1566442796700_717 singleImage clk" data-href="https://s3.nzbdw.com/s?type=2&amp;r=20&amp;mv_ref=blog.csdn.net&amp;enup=CAABPZZFTQgAAk1Flj0A&amp;mvid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;bid=138b26aa9a90a369&amp;price=AAAAAF1eBSoAAAAAAAfbqX6fMz8zM1GYn146iA==&amp;finfo=DAABCAABAAAAYQgAAgAAAC0EAAM/YO9ms67EtQAIAAIAAAADCgADTvwAarR6HTYIAAQAAAAvBgAGLbcGAAoAAAYADD64CAAOAAAAHQoADwAAAAAAEHrCAA&amp;ugi=FcTgigEVyNtrTBUCFc4EFdoEFQAAFeGl/PUIFoQIFcgBFoD64KT8qsgFHBaNy7+FvZqH/8wBFQAAAA&amp;uai=FeTGlAIlAhUEFoCo1s20jvf7nQEV8ggllOq16A0lABUaFAAcFuPHo72Kx6qTzgEVAAAA&amp;ubi=FY7hQxW8lu8CFYyP+RgVzpmKXBUEFRwWsN6Y3BYWgKiAnMiagPydATQEFrDgkIAIJQYVjcSC6QsVwgUVADaW7ra7hIyXkPUBAA&amp;clickid=0&amp;cpx=__OFFSET_X__&amp;cpy=__OFFSET_Y__&amp;cs=__EVENT_TIME_START__&amp;ce=__EVENT_TIME_END__&amp;csign2=kUBAKWT_R5o=&amp;url=http%3A%2F%2Fmuscdlb.bceapp.com%2F2010xls%2Fm2%2F" data-pv="https://s3.nzbdw.com/s?type=1&amp;r=20&amp;tid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;finfo=DAABCAABAAAAYQgAAgAAAC0EAAM/YO9ms67EtQAIAAIAAAADCgADTvwAarR6HTYIAAQAAAAvBgAGLbcGAAoAAAYADD64CAAOAAAAHQoADwAAAAAAEHrCAA&amp;mv_ref=blog.csdn.net&amp;enup=CAABPZZFTQgAAk1Flj0A&amp;mvid=NzQyMDc0MDQyMTMwNTEyMjQwMjAwMTk&amp;bid=138b26aa9a90a369&amp;ugi=FcTgigEVyNtrTBUCFc4EFdoEFQAAFeGl/PUIFoQIFcgBFoD64KT8qsgFHBaNy7+FvZqH/8wBFQAAAA&amp;uai=FeTGlAIlAhUEFoCo1s20jvf7nQEV8ggllOq16A0lABUaFAAcFuPHo72Kx6qTzgEVAAAA&amp;ubi=FY7hQxW8lu8CFYyP+RgVzpmKXBUEFRwWsN6Y3BYWgKiAnMiagPydATQEFrDgkIAIJQYVjcSC6QsVwgUVADaW7ra7hIyXkPUBAA&amp;ds=2&amp;price=AAAAAF1eBSoAAAAAAAfbqX6fMz8zM1GYn146iA==,https://max-l.mediav.com/rtb?type=2&amp;ver=1&amp;v=CGQSEDEzOGIyNmFhOWE5MGEzNjkYsqOKASCisEUoAmIXNzQyMDc0MDQyMTMwNTEyMjQwMjAwMTmIAQA&amp;k=XPdRDAAAAAA=&amp;w=AAAAAF1eBSoAAAAAAAfb4Gmmo-NlSscYeW0g3A&amp;i=5zwvYcy7TDh0&amp;exp=BQBECgBEAQJEBAJEEABDIgBD&amp;z=1" data-clk="https://max-l.mediav.com/rtb?type=3&amp;ver=1&amp;v=CGQSEDEzOGIyNmFhOWE5MGEzNjkYsqOKASCisEUoAmIXNzQyMDc0MDQyMTMwNTEyMjQwMjAwMTlwAA&amp;k=1tSj6gAAAAA=&amp;i=5zwvYcy7TDh0&amp;exp=BQBECgBEAQJEBAJEEABDIgBD&amp;x=__OFFSET_X__&amp;y=__OFFSET_Y__&amp;st=__EVENT_TIME_START__&amp;et=__EVENT_TIME_END__&amp;adw=__ADSPACE_W__&amp;adh=__ADSPACE_H__&amp;tc=&amp;turl=">
    <info-div class="wrap">
        <info-div class="singleImage-img singleImage-img-left">
            <info-div class="img" style="background-image:url(https://s3m.nzwgs.com/galileo/bfddcc01e23433460023d950b9c7911d.gif)"><info-div class="ads-tag"></info-div></info-div>
        </info-div>
        <info-div class="singleImage-body singleImage-body-left">
            <info-div class="singleImage-title">空姐说:99元体验男朋友轻松延长40分钟,多吃它,你也行!</info-div>
            <info-div class="singleImage-desc">梦雨 · 猎媒</info-div>
        </info-div>
    

    【教程】“视频教程:卡尔曼滤波器的原理以及在MATLAB中的实现”原代码在matlab下不显示曲线的解决办法

    05-13 阅读数 825

    一、先看原视频教程来源:原博客地址(@山猫小队长): 视频地址整理:优酷:http://v.youku.com/v_show/id_XNzQwMTMwOTQ0.htmlB站:https://www.b... 博文 来自: 柠檬很酸的博客

    matlab 实现语音增强(含维纳滤波、谱减法、卡尔曼滤波三种实现方法)下载

    05-19

    有完整的代码注释,三种方法都可以实现,并有demo音频。 相关下载链接://download.csdn.net/download/everock/10424395?utm_source=bbsseo 论坛

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/bbluce99/1949738&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;43&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/bbluce99/1949738&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;43&quot;}">
    	<a href="https://download.csdn.net/download/bbluce99/1949738" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					kalman滤波、平滑和预测 MATLAB程序					</h4>
    				<span class="data float-right">12-30</span>
    			</div>
    			<div class="desc oneline">
    					编的很完善的kalman滤波、平滑以及预测MATLAB程序。希望对大家有用,尤其是在校大学生。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_67" data-pid="67" data-report-view="{&quot;mod&quot;:&quot;kp_popu_67-658&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_67-658&quot;,&quot;keyword&quot;:&quot;&quot;}"><script type="text/javascript">
    (function() {
        var s = "_" + Math.random().toString(36).slice(2);
        document.write('<div style="" id="' + s + '"></div>');
        (window.slotbydup = window.slotbydup || []).push({
            id: "u3573058",
            container:  s
        });
    })();
    

    <div class="recommend-item-box recommend-box-ident recommend-download-box clearfix" data-report-view="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/Inuyasha19/2035348&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;44&quot;}" data-report-click="{&quot;mod&quot;:&quot;popu_614&quot;,&quot;dest&quot;:&quot;https://download.csdn.net/download/Inuyasha19/2035348&quot;,&quot;strategy&quot;:&quot;BlogCommendFromBaidu&quot;,&quot;index&quot;:&quot;44&quot;}">
    	<a href="https://download.csdn.net/download/Inuyasha19/2035348" target="_blank">
    		<div class="content clearfix">
    			<div class="">
    				<h4 class="text-truncate oneline clearfix">
    					基于<em>卡尔曼滤波</em>的目标跟踪matlab经典程序——快速入门					</h4>
    				<span class="data float-right">01-30</span>
    			</div>
    			<div class="desc oneline">
    					基于卡尔曼滤波的目标跟踪经典程序,用于2维目标的跟踪,是初学者学习卡尔曼滤波的好教程。深入浅出,易于理解。				</div>
    			<span class="type-show type-show-download">下载</span>
    		</div>
    	</a>
    </div>
    
    <div class="recommend-item-box recommend-ad-box"><div id="kp_box_68" data-pid="68" data-report-view="{&quot;mod&quot;:&quot;kp_popu_68-625&quot;,&quot;keyword&quot;:&quot;&quot;}" data-report-click="{&quot;mod&quot;:&quot;kp_popu_68-625&quot;,&quot;keyword&quot;:&quot;&quot;}"><div style="width: 100%;"><span style="display:none;padding-left:0px;"></span><iframe width="852" frameborder="0" height="60" scrolling="no" src="https://pos.baidu.com/s?hei=60&amp;wid=852&amp;di=u3565460&amp;ltu=https%3A%2F%2Fblog.csdn.net%2Fzhangquan2015%2Farticle%2Fdetails%2F79264540&amp;psi=1f283c47c9792b12ed499c54965f226d&amp;tlm=1566442796&amp;chi=1&amp;dai=8&amp;pcs=1858x920&amp;dis=0&amp;par=1920x1040&amp;drs=1&amp;ti=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E5%BA%94%E7%94%A8%E5%8F%8A%E5%85%B6matlab%E5%AE%9E%E7%8E%B0&amp;ltr=https%3A%2F%2Fblog.csdn.net%2FAsc11_%2Farticle%2Fdetails%2F40714239&amp;cfv=0&amp;exps=111000,118009,110011&amp;cpl=4&amp;tcn=1566442797&amp;dri=0&amp;cdo=-1&amp;cmi=6&amp;tpr=1566442796618&amp;ccd=24&amp;pis=-1x-1&amp;cce=true&amp;cec=UTF-8&amp;col=zh-CN&amp;dtm=HTML_POST&amp;dc=3&amp;psr=1920x1080&amp;ps=47205x580&amp;pss=1858x47259&amp;ant=0&amp;cja=false&amp;prot=2&amp;ari=2"></iframe></div><script type="text/javascript" src="//rabc1.iteye.com/common/openjs/m022.js?hcuzbzy=bi"></script></div></div>
    
                            <div class="recommend-loading-box">
                <img src="https://csdnimg.cn/release/phoenix/images/feedLoading.gif">
            </div>
            <div class="recommend-end-box" style="display: block;">
                <p class="text-center">没有更多推荐了,<a href="https://blog.csdn.net/" class="c-blue c-blue-hover c-blue-focus">返回首页</a></p>
            </div>
        </div>
    </main>
    
    展开全文
  • 抗差自适应,抗差自适应卡尔曼滤波,matlab源码
  • 自适应卡尔曼滤波模型的MATLAB编程实现.rar
  • 采用最优估计理论中卡尔曼滤波理论基础进行系统的研究,建立了组合导航系统状态方程和观测方程,给出了GPS/INS组合导航模型。通过对GPS/INS位置组合导航系统用Kalman滤波和自适应Kalman分析,并应用MATLAB 软件仿真...
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...

    更新日志:

    2020.02.13:修改了第三节推导中的公式错误

    2020.03.21:修改了2.1节中的部分表述和公式加粗,补充迹的求导公式

    2021.04.14:修改公式显示错误

    1 简介

    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。

    EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用卡尔曼滤波框架对信号进行滤波,因此它是一种次优滤波。

    2 算法介绍

    2.1 泰勒级数展开

    泰勒级数展开是将一个在x=x_{0}处具有n阶导数的函数f(x),利用关于(x-x_{0})n次多项式逼近函数值的方法。

    若函数f(x)在包含x_{0}的某个闭区间[a,b]上具有n阶导数,且在开区间(a,b)上具有(n+1)阶导数,则对闭区间[a,b]上的任意一点x,都有:

    f(x)=\frac{f({​{x}_{0}})}{0!}+\frac{f'({​{x}_{0}})}{1!}(x-{​{x}_{0}})+...+\frac{​{​{f}^{(n)}}({​{x}_{0}})}{n!}{​{(x-{​{x}_{0}})}^{n}}+{​{R}_{n}}(x)

    其中{​{f}^{(n)}}({​{x}_{0}})表示函数f(x)x=x_{0}处的n阶导数,等式右边成为泰勒展开式,剩余的{​{R}_{n}}(x)是泰勒展开式的余项,是(x-x_{0})^{n}的高阶无穷小。

    (著名的欧拉公式{​{e}^{ix}}=\cos x +i\sin x就是利用{​{e}^{ix}}\cos x\sin x的泰勒展开式得来的!)

    当变量是多维向量时,一维的泰勒展开就需要做拓展,具体形式如下:

    f(\mathbf{x})=f({​{\mathbf{x}}_{k}})+{​{[\nabla f({​{\mathbf{x}}_{k}})]}^{T}}(\mathbf{x}-{​{\mathbf{x}}_{k}})+{​{o}^{n}}

    其中,{​{[\nabla f({​{\mathbf{x}}_{k}})]}^{T}}={​{\mathbf{J}}({\bf x}_k)}表示雅克比矩阵,{​{\mathbf{o}}^{n}}表示高阶无穷小。

    {[\nabla f({​{\bf{x}}})]^T} = {​{\bf{J}}({\bf x})} = \begin{bmatrix} \frac{\partial f({\bf x})_1}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_1}{\partial {\bf x}_n}\\ \vdots & \ddots & \vdots \\ \frac{\partial f({\bf x})_m}{\partial {\bf x}_1} & \hdots & \frac{\partial f({\bf x})_m}{\partial {\bf x}_n} \end{bmatrix}

    这里,f({​{\bf{x}}_k})m维,{​{\bf{x}}_k}状态向量为n维,\frac{​{​{\partial ^2}f({​{\bf{x}}_k})}}{​{\partial {x_m}\partial {x_n}}} = \frac{​{\partial f({​{\bf{x}}_k})^T}}{​{\partial {x_m}}}\frac{​{\partial f({​{\bf{x}}_k})}}{​{\partial {x_n}}}

    一般来说,EKF在对非线性函数做泰勒展开时,只取到一阶导和二阶导,而由于二阶导的计算复杂性,更多的实际应用只取到一阶导,同样也能有较好的结果。取一阶导时,状态转移方程和观测方程就近似为线性方程,高斯分布的变量经过线性变换之后仍然是高斯分布,这样就能够延用标准卡尔曼滤波的框架。

    2.1 EKF

    标准卡尔曼滤波KF的状态转移方程和观测方程为

    {​{\mathbf{\theta }}_{k}}=\mathbf{A}{​{\mathbf{\theta }}_{k-1}}+\mathbf{B}{​{\mathbf{u}}_{k-1}}+{​{\mathbf{s}}_{k}}

    {​{\mathbf{z}}_{k}}=\mathbf{C}{​{\mathbf{\theta }}_{k}}+{​{\mathbf{v}}_{k}}

    扩展卡尔曼滤波EKF的状态转移方程和观测方程为

    {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}})+{​{\mathbf{s}}_{k}}          (1)

    {​{\mathbf{z}}_{k}}=h({​{\mathbf{\theta }}_{k}})+{​{\mathbf{v}}_{k}}             (2)

    利用泰勒展开式对(1)式在上一次的估计值\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle处展开得

    {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}})+{​{\mathbf{s}}_{k}}=f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle )+{​{\mathbf{F}}_{k-1}}\left( {​{\mathbf{\theta }}_{k-1}}-\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle \right)+{​{\mathbf{s}}_{k}}          (3)

    再利用泰勒展开式对(2)式在本轮的状态预测值\mathbf{\theta }_{k}^{'}处展开得

    {​{\mathbf{z}}_{k}}=h({​{\mathbf{\theta }}_{k}})+{​{\mathbf{v}}_{k}}=h\left( \mathbf{\theta }_{​{k}}^{\mathbf{'}} \right)+{​{\mathbf{H}}_{k}}\left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{​{k}}^{\mathbf{'}} \right)+{​{\mathbf{v}}_{k}}            (4)

    其中,{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别表示函数f(\mathbf{\theta })h(\mathbf{\theta })\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle\mathbf{\theta }_{k}^{'}处的雅克比矩阵。

    (注:这里对泰勒展开式只保留到一阶导,二阶导数以上的都舍去,噪声假设均为加性高斯噪声)

    基于以上的公式,给出EKF的预测(Predict)和更新(Update)两个步骤:

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{​{\mathbf{\Sigma }}_{k-1}}{​{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={​{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {​{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {​{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {​{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    其中的雅克比矩阵{\mathbf{F}}_{k-1}{\mathbf{H}}_{k}分别为

    {​{\mathbf{F}}_{k-1}}={​{\left. \frac{\partial f}{\partial \mathbf{\theta }} \right|}_{\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle }}{​{\mathbf{H}}_{k}}={​{\left. \frac{\partial h}{\partial \mathbf{\theta }} \right|}_{\mathbf{\theta }_{k}^{'}}}

    雅可比矩阵的计算,在MATLAB中可以利用对自变量加上一个eps(极小数),然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。

    读者可能好奇?为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢?以下做一个简单的推导。

    3 推导

    先列出几个变量的表示、状态转移方程和观测方程:

    真实值{​{\mathbf{\theta }}_{k}},预测值\mathbf{\theta }_{k}^{'},估计值\left\langle {​{\mathbf{\theta }}_{k}} \right\rangle,观测值{​{\mathbf{z}}_{k}},观测值的预测\mathbf{\hat{z}}_{k},估计值与真实值之间的误差协方差矩阵{​{\mathbf{\Sigma }}_{k}},求期望的符号\left\langle \cdot \right\rangle

    {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}})+{​{\mathbf{s}}_{k}},     {​{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})

    {​{\mathbf{z}}_{k}}=h({​{\mathbf{\theta }}_{k}})+{​{\mathbf{v}}_{k}},     {​{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})

    引入反馈:\left\langle {​{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{​{\mathbf{K}}_{k}}\left( {​{\mathbf{z}}_{k}}-{​{​{\mathbf{\hat{z}}}}_{k}} \right)=\mathbf{\theta }_{k}^{'}+{​{\mathbf{K}}_{k}}\left( {​{\mathbf{z}}_{k}}-h(\theta _{k}^{'} )\right)      (5)

    OK,可以开始推导了:

    由公式(3)(4)得到以下两个等式,标为式(6)(7)

    f({​{\mathbf{\theta }}_{k-1}})-f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle )={​{\mathbf{F}}_{k-1}}\left( {​{\mathbf{\theta }}_{k-1}}-\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle \right)

    h({​{\mathbf{\theta }}_{k}})-h\left( \mathbf{\theta }_{​{k}}^{\mathbf{'}} \right)={​{\mathbf{H}}_{k}}\left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{​{k}}^{​{'}} \right)

    计算估计值与真实值之间的误差协方差矩阵{​{\mathbf{\Sigma }}_{k}},并把式子(4)(5)(7)代入,得到

    {​{\mathbf{\Sigma }}_{k}}=\left\langle {​{\mathbf{e}}_{k}}\mathbf{e}_{k}^{T} \right\rangle =\left\langle \left( {​{\mathbf{\theta }}_{k}}-\left\langle {​{\mathbf{\theta }}_{k}} \right\rangle \right){​{\left( {​{\mathbf{\theta }}_{k}}-\left\langle {​{\mathbf{\theta }}_{k}} \right\rangle \right)}^{T}} \right\rangle

    {​{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{​{\mathbf{K}}_{k}}\left( {​{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]{​{\left[ {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{​{\mathbf{K}}_{k}}\left( {​{\mathbf{z}}_{k}}-h\left( \mathbf{\theta }_{k}^{'} \right) \right) \right]}^{T}} \right\rangle

    {​{\mathbf{\Sigma }}_{k}}=\left\langle \left[ {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{​{\mathbf{K}}_{k}}\left( h\left( {​{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{​{\mathbf{v}}_{k}} \right) \right]{​{\left[ {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'}-{​{\mathbf{K}}_{k}}\left( h\left( {​{\mathbf{\theta }}_{k}} \right)-h\left( \mathbf{\theta }_{k}^{'} \right)+{​{\mathbf{v}}_{k}} \right) \right]}^{T}} \right\rangle

    {​{\mathbf{\Sigma }}_{k}}=\left\langle \left[ \left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right)\left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{\mathbf{K}}_{k}{\mathbf{v}}_{k} \right] \left[ \left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right)\left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)+{\mathbf{K}}_{k}{\mathbf{v}}_{k} \right]^T \right\rangle

    {​{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right)\left\langle \left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right){​{\left( {​{\mathbf{\theta }}_{k}}-\mathbf{\theta }_{k}^{'} \right)}^{T}} \right\rangle {​{\left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}{\mathbf{R}}{\mathbf{K}}_{k}^{T}

    {​{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right) \mathbf{\Sigma }_{k}^{'}{​{\left( \mathbf{I}-{​{\mathbf{K}}_{k}}{​{\mathbf{H}}_{k}} \right)}^{T}}+{\mathbf{K}}_{k}{\mathbf{R}}{\mathbf{K}}_{k}^{T}

    其中\mathbf{\Sigma }_{k}^{'}表示真实值与与预测值之间的误差协方差矩阵。于是得到式(8)

    {​{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-{​{\mathbf{K}}_{k}}\mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}_{k}^{T}}}\mathbf{K}_{k}^{T}+{​{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T}

    因为{​{\mathbf{\Sigma }}_{k}}的对角元即为真实值与估计值的误差的平方,矩阵的迹(用T[]表示)即为总误差的平方和,即

    T\left[ {​{\mathbf{\Sigma }}_{k}} \right]=T\left[ \mathbf{\Sigma }_{k}^{'} \right]+T\left[ {​{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma } }_{k}^{'}{​{\mathbf{H}_{k}}^{T}}+\mathbf{R} \right)\mathbf{K}_{k}^{T} \right]-2T\left[ {​{\mathbf{K}}_{k}}\mathbf{H}_{k}\mathbf{\Sigma }_{k}^{'} \right]

    利用以下矩阵迹的求导公式(其中\bf A\bf B表示矩阵,\bf a表示列向量):

    Tr(\mathbf{A}+\mathbf{B})=Tr(\mathbf{A})+Tr(\mathbf{B})

    Tr(\mathbf{AB})=Tr(\mathbf{BA})

    \mathbf{a}^{T} \mathbf{a}=Tr(\mathbf{a}\mathbf{a}^{T})

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XBX}^{T})=\mathbf{XB}^{T}+\mathbf{XB}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{AX}^{T})=\mathbf{A}

    \frac{\partial }{\partial \mathbf{X}} Tr(\mathbf{XA})=\mathbf{A}^{T}

    要让估计值更接近于真实值,就要使上面的迹尽可能的小,因此要取得合适的卡尔曼增益{​{\mathbf{K}}_{k}},使得迹得到最小,言外之意就是使得迹对{​{\mathbf{K}}_{k}}的偏导为0,即

    \frac{dT\left[ {​{\mathbf{\Sigma }}_{k}} \right]}{d{​{\mathbf{K}}_{k}}}=2{​{\mathbf{K}}_{k}}\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'}{​{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)-2{​{\left( \mathbf{H}_{k}{\mathbf{\Sigma }}_{k}^{'} \right)}^{T}}=0

    这样就能算出合适的卡尔曼增益了,即

    {​{\mathbf{K}}_{k}}=\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}{​{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    代回式(8)得到

    {​{\mathbf{\Sigma }}_{k}}=\mathbf{\Sigma }_{k}^{'}-\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}{​{\left( \mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}\mathbf{H}_{k}{\mathbf\Sigma }_{k}^{'}=\left( \mathbf{I}-{​{\mathbf{K}}_{k}}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    接下来就差真实值与预测值之间的协方差矩阵\mathbf{\Sigma }_{k}^{'}的求值公式了

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \mathbf{e}_{k}^{'}\mathbf{e}{​{_{k}^{'}}^{T}} \right\rangle =\left\langle \left( {​{\theta }_{k}}-\theta _{k}^{'} \right){​{\left( {​{\theta }_{k}}-\theta _{k}^{'} \right)}^{T}} \right\rangle

    将以下两个等式代入到上式,

    {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}})+{​{\mathbf{s}}_{k}}\mathbf{\theta }_{k}^{'}=f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle )

    可以得到

    \mathbf{\Sigma }_{_{k}}^{'}=\left\langle \left[f({​{\mathbf{\theta }}_{k-1}})-f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle )+{​{\mathbf{s}}_{k}} \right]{​{\left[ f({​{\mathbf{\theta }}_{k-1}})-f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle )+{​{\mathbf{s}}_{k}} \right]}^{T}} \right\rangle

    {​{\mathbf{\theta }}_{k}}\left\langle {​{\mathbf{\theta }}_{k}} \right\rangle与观测噪声{​{\mathbf{s}}_{k}}是独立的,求期望等于零;;\left\langle {​{\mathbf{s}}_{k}}{​{\mathbf{s}}_{k}}^{T} \right\rangle表示观测噪声的协方差矩阵,用{\mathbf{Q}}表示。于是得到

    \mathbf{\Sigma }_{_{k}}^{'}=\mathbf{F}_{k-1}\left\langle \left( {​{\theta }_{k-1}}-\left\langle {​{\theta }_{k-1}} \right\rangle \right){​{\left( {​{\theta }_{k-1}}-\left\langle {​{\theta }_{k-1}} \right\rangle \right)}^{T}} \right\rangle {​{\mathbf{F}}_{k-1}^{T}}+\left\langle {​{\mathbf{s}}_{k}}\mathbf{s}_{k}^{T} \right\rangle \\ =\mathbf{F}_{k-1}{​{\mathbf{\Sigma }}_{k-1}}{​{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    其中的协方差矩阵的转置矩阵就是它本身。OK,大功告成,以上就完成了全部公式的推导了。

    4 实际应用

    现在我们假设在海上有一艘正在行驶的船只,其相对于传感器的横纵坐标为(x;y;v_{x};v_{y})为隐藏状态,无法直接获得,而传感器可以测量得到船只相对于传感器的距离和角度(r;\theta),传感器采样的时间间隔为\Delta t,则:

    状态向量(x;y;v_{x};v_{y}),观测向量(r;\theta)

    状态转移方程和观测方程为:

    \left[ \begin{matrix} {​{x}_{k}} \\ {​{y}_{k}} \\ {​{v}_{x_{k}}} \\ {​{v}_{y_{k}}} \\ \end{matrix} \right]=f(\left[ \begin{matrix} {​{x}_{k-1}} \\ {​{y}_{k-1}} \\ {​{v}_{​{​{x}_{k-1}}}} \\ {​{v}_{​{​{y}_{k-1}}}} \\ \end{matrix} \right])+{​{\mathbf{s}}_{k}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {​{x}_{k-1}} \\ {​{y}_{k-1}} \\ {​{v}_{​{​{x}_{k-1}}}} \\ {​{v}_{​{​{y}_{k-1}}}} \\ \end{matrix} \right]+{​{\mathbf{s}}_{k}}

    \left[ \begin{matrix} {​{r}_{k}} \\ {​{\theta }_{k}} \\ \end{matrix} \right]=h(\left[ \begin{matrix} {​{x}_{k}} \\ {​{y}_{k}} \\ {​{v}_{xk}} \\ {​{v}_{yk}} \\ \end{matrix} \right])+{​{\mathbf{v}}_{k}}=\left[ \begin{matrix} \sqrt{x_{k}^{2}+x_{y}^{2}} \\ \arctan \frac{​{​{y}_{k}}}{​{​{x}_{k}}} \\ \end{matrix} \right]+{​{\mathbf{v}}_{k}}

    那么雅克比矩阵为

    {​{\mathbf{F}}_{k-1}}=\left[ \begin{matrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]

    {​{H}_{k}}=\left[ \begin{matrix} \frac{\partial {​{r}_{k}}}{\partial {​{x}_{k}}} & \frac{\partial {​{r}_{k}}}{\partial {​{y}_{k}}} & \frac{\partial {​{r}_{k}}}{\partial {​{v}_{​{​{x}_{k}}}}} & \frac{\partial {​{r}_{k}}}{\partial {​{v}_{​{​{y}_{k}}}}} \\ \frac{\partial {​{\theta }_{k}}}{\partial {​{x}_{k}}} & \frac{\partial {​{\theta }_{k}}}{\partial {​{y}_{k}}} & \frac{\partial {​{\theta }_{k}}}{\partial {​{v}_{​{​{x}_{k}}}}} & \frac{\partial {​{\theta }_{k}}}{\partial {​{v}_{​{​{y}_{k}}}}} \\ \end{matrix} \right]

    这里给定距离传感器的噪声均值为0,方差为10;角度传感器的噪声均值为0,方差为0.001(单位弧度);

    采样时间点为\small 100个;

    船只的初始状态为(1000;1500;5;-3),四个状态量的噪声的方差分别为(2;2;0.2;0.2)。仿真结果如下:

    从仿真结果可以看出,EKF在这种情形下的滤波效果还是不错的,但是在实际应用中,对于船只运动的状态转移噪声的均值\mathbf q和协方差矩阵\mathbf Q,以及传感器的观测噪声的均值\mathbf r和协方差矩阵\mathbf R,往往都是未知的,有很多情况都只有观测值而已,这样的情形下,就有必要利用观测值对噪声的统计量参数做出适当的估计(学习)。

    5 参数估计(参数学习)

    利用EM算法和极大后验概率估计(MAP),对未知的噪声参数做出估计,再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出,之后可能会补充,这里就先给出一种Adaptive-EKF算法的公式。

    {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}})+{​{\mathbf{s}}_{k}},     {​{\mathbf{s}}_{k}}\sim \mathcal{N}(\mathbf{q},\mathbf{Q})

    {​{\mathbf{z}}_{k}}=h({​{\mathbf{\theta }}_{k}})+{​{\mathbf{v}}_{k}},     {​{\mathbf{v}}_{k}}\sim \mathcal{N}(\mathbf{r},\mathbf{R})

    {​{\mathbf{\varepsilon }}_{k}}={​{\mathbf{z}}_{k}}-h(\mathbf{\theta }_{k}^{'})-{​{\mathbf{r}}_{k}}

    (1)E-Step

    Propagation:

    \mathbf{\theta }_{k}^{'}=f(\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle)

    \mathbf{\Sigma }_{k}^{'}=\mathbf{F}_{k-1}{​{\mathbf{\Sigma }}_{k-1}}{​{\mathbf{F}}_{k-1}^{T}}+\mathbf{Q}

    Update:

    \mathbf{S}_{k}^{'}={​{\left( \mathbf{H_{k}\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}+\mathbf{R} \right)}^{-1}}

    \mathbf{K}_{k}^{'}=\mathbf{\Sigma }_{k}^{'}{​{\mathbf{H}}_{k}^{T}}\mathbf{S}_{k}^{'}

    \left\langle {​{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+\mathbf{K}_{k}^{'}\left( {​{\mathbf{z}}_{k}}-{h(\theta }_{k}^{'}) \right)

    {​{\mathbf{\Sigma }}_{k}}=\left( \mathbf{I}-\mathbf{K}_{k}^{'}\mathbf{H}_{k} \right)\mathbf{\Sigma }_{k}^{'}

    (2)M-Step

    {​{\mathbf{\hat{q}}}_{k}}=\left( 1-\frac{1}{k} \right){​{\mathbf{\hat{q}}}_{k\text{-}1}}+\frac{1}{k}\left[ \left\langle {​{\theta }_{k}} \right\rangle -f\left( \left\langle {​{\theta }_{k-1}} \right\rangle \right) \right]

    {​{\mathbf{\hat{Q}}}_{k}}=\left( 1-\frac{1}{k} \right){​{\mathbf{\hat{Q}}}_{k\text{-}1}}+\frac{1}{k}\left[ {​{\mathbf{K}}_{k}}{​{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}\mathbf{K}_{k}^{T}+{​{\mathbf{\Sigma }}_{k}}-{​{\mathbf{F}}_{k-1}}{​{\mathbf{\Sigma }}_{k-1}}\mathbf{F}_{k-1}^{T} \right]

    {​{\mathbf{\hat{r}}}_{k}}=\left( 1-\frac{1}{k} \right){​{\mathbf{\hat{r}}}_{k\text{-}1}}+\frac{1}{k}\left[ {​{\mathbf{z}}_{k}}-h\left( \left\langle \theta _{k}^{'} \right\rangle \right) \right]

    {​{\mathbf{\hat{R}}}_{k}}=\left( 1-\frac{1}{k} \right){​{\mathbf{\hat{R}}}_{k\text{-}1}}+\frac{1}{k}\left[ {​{\mathbf{\varepsilon }}_{k}}\mathbf{\varepsilon }_{k}^{T}-{​{\mathbf{H}}_{k}}\mathbf{\Sigma }_{k}^{'}\mathbf{H}_{k}^{T} \right]

    利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪,得到的效果如下图所示:

    相比于没有做参数估计的EKF滤波,可以看出,Adaptive-EKF在估计误差上要优于EKF滤波,而且,它并不需要指定状态转移噪声和观测噪声的参数,将更有利于在实际中的应用。

    6 总结

    EKF滤波通过泰勒展开公式,把非线性方程在局部线性化,使得高斯分布的变量在经过线性变换后仍然为高斯分布,这使得能继续把标准卡尔曼滤波KF的框架拿过来用,总体来说,EKF在函数的非线性不是很剧烈的情形下,能够具有很不错的滤波效果。但是EKF也有它的不足之处:其一,它必须求解非线性函数的Jacobi矩阵,对于模型复杂的系统,它比较复杂而且容易出错;其二,引入了线性化误差,对非线性强的系统,容易导致滤波结果下降。基于以上原因,为了提高滤波精度和效率,以满足特殊问题的需要,就必须寻找新的逼近方法,于是便有了粒子滤波PF和无迹卡尔曼滤波UKF,笔者将在接下来的博文中为读者解读。

    7 参考文献

    [1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37. 

    [2] https://www.cnblogs.com/gaoxiang12/p/5560360.html.

    [3] https://max.book118.com/html/2017/0502/103920556.shtm.


    原创性声明:本文属于作者原创性文章,小弟码字辛苦,转载还请注明出处。谢谢~ 

    如果有哪些地方表述的不够得体和清晰,有存在的任何问题,亦或者程序存在任何考虑不周和漏洞,欢迎评论和指正,谢谢各路大佬。

    需要代码和有需要相关技术支持的可咨询QQ:297461921

    展开全文
  • 针对“当前”统计模型中预先设置机动频率和加速度极限值造成对目标跟踪精度不高的问题, 提出一种新的参数自适应算法? 该算法利用目标前后2个时刻的加速度均值代替“当前”统计模型中只利用前一时刻的加速度值作为...
  • 卡尔曼滤波应用及其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。 假定快时刻...

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

    展开全文
  • 抗差卡尔曼滤波

    2015-09-17 16:38:09
    基于双因子抗差权的KALMAN滤波模型研究,里面有具体文章供参考,谢谢原作者!
  • 卡尔曼滤波Matlab实现

    万次阅读 多人点赞 2018-08-07 01:19:53
    卡尔曼滤波原理浅析 一、什么是卡尔曼滤波  滤波是从信号中提取有用信息的过程,比如从电信号中提取有用的频谱分量,从观测到的物体轨迹中提取位置信息,滤除图像信号中的噪声等。卡尔曼滤波是一种有效的滤波...
  • 针对异步电机直接转矩控制低速转矩脉动大的问题,提出了一种自适应强跟踪有限微分扩展卡尔曼滤波算法(STFDEKF)。该算法采用多项式近似技术和一阶中心差分法计算非线性函数的偏导数,它具有二阶非线性近似的能力;同时...
  • 需要注意:卡尔曼滤波模块的输入包括两部分:控制对象的观测值y以及对象的输入u,这两部分都包含相应的干扰。 其中matlab function模块的代码: function y_filter = fcn ( u ) persistent A B C D Q p R ...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 430
精华内容 172
关键字:

自适应卡尔曼滤波matlab

matlab 订阅