Java教程

UKF详解

本文主要是介绍UKF详解,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!

文章来源https://blog.csdn.net/lpccclp/article/details/115333818

UKF匀速直线运动目标跟踪

  • 无迹卡尔曼滤波
  • 运行结果

无迹卡尔曼滤波

代码:

function UKF
clc;
clear;
T=1;           % 采样周期
N=60/T;        % 采样次数
X=zeros(4,N);  % 初始化真实轨迹矩阵
X(:,1)=[100 2 200 20]; % 目标初始位置、速度
Z=zeros(1,N);  % 初始化 观测距离矩阵
w =1e-5;
Q = 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];% 状态转移矩阵
Xstation=[200,300];  % 观测位置
V = sqrt(R)*randn(1,N);    % v均值为0,协方差为R的高斯白噪声 %sqrt(A)矩阵每个元素分别开方,与矩阵点乘有关;sqrtm(A)矩阵为整体,与矩阵相乘有关
W = sqrt(Q)*randn(2,1);    % W均值为0,协方差为Q的高斯白噪声
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N 
    X(:,t)=F*X(:,t-1)+G*W; % 目标的真实轨迹
end
for t=1:N
    Z(t)=Distance(X(:,t),Xstation)+V(t);  % 真实观测目标位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%% UKF滤波 %%%%%%%%%%%%%%%%%%
L=4;     % L为状态维度
a=1;     % a控制采样点的分布状态
k=1;     % k为待选参数 没有界限,但是要保证(n+lamda)*P为半正定矩阵
b=2;     % b非负的权系数
lambda=a*a*(L+k)-L; % lambda为缩放比列参数,用于降低总的预测误差
%%%%%%%%%%%%%%%%%%%%%% sigma点的权值 %%%%%%%%%%%%%%%%%
Wm=zeros(1,2*L+1);
Wc=zeros(1,2*L+1);
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-a*a+b; % 第一个采样点的均值和协方差的权值
for j=2:2*L+1      % 剩下2n个sigma点的均值和协方差的权值
    Wm(j)= lambda/(2*(L+lambda));  % 下标m为均值的权值
    Wc(j)= lambda/(2*(L+lambda));  % 下标c为协方差的权值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf = zeros(4,N);
Xukf(:,1)=X(:,1);  %无迹Kalman滤波状态初始化
P0=eye(4);         %协方差阵初始化
for t=2:N
    xestimate = Xukf(:,t-1);
    P=P0;
%%%%%%% 第一步:获得一组采样点,Sigma点集 %%%%%%%%%%%%%%
    cho=(chol(P*(L+lambda)))';  % cho=chol(X)用于对矩阵X进行Cholesky分解,产生一个上三角阵cho,使cho'cho=X。若X为非对称正定,则输出一个出错信息。
    for k=1:L
        xgamaP1(:,k)=xestimate+cho(:,k);
        xgamaP2(:,k)=xestimate-cho(:,k);
    end
    Xsigma=[xestimate xgamaP1 xgamaP2]; % 获得 2L+1 个Sigma点
%第二步:对Sigme点集进行一步预测
    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+lambda)*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)=h(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
%第七步:计算Kalman增益
    K=Pxz*inv(Pzz);  % Kalman
%第八步:状态和方差更新
    Xukf(:,t)=Xpred+K*(Z(t)-Zpred);% 状态更新
    P=Ppred-K*Pzz*K'; %方差更新
    P0=P;
end
%6误差分析
Err_KalmanFilter=zeros(2,N);
for i=1:N
    Err_KalmanFilter(1,i)=X(1,i)-Xukf(1,i); %滤波后的误差
    Err_KalmanFilter(2,i)=X(3,i)-Xukf(3,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),'-B.');       %真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-rs'); %无迹Kalman滤波轨迹
legend('真实轨迹','UKF轨迹')
xlabel('横向坐标/m')
ylabel('纵向坐标');
figure
hold on;
box on;
plot(Err_KalmanFilter(1,:),'-kd','MarkerFace','r');
xlabel('时间/s');
ylabel('横向坐标误差/m');
figure
plot(Err_KalmanFilter(2,:),'-bd','MarkerFace','g');
xlabel('时间/s');
ylabel('纵向坐标误差/m');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%子函数:求两点间的距离
function d=Distance(X1,X2)
    d=sqrt((X1(1)- X2(1))^2 + (X1(3)-X2(2))^2);
end
%观测子函数:观测距离
function [y]=h(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
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
  • 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

运行结果

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

参考:参考书籍:卡尔曼滤波原理及应用

这篇关于UKF详解的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!