├── 1 ├── code_1 │ ├── LSM.m │ ├── MVE.m │ ├── WLSM.m │ └── main123.m ├── code_2 │ ├── FIR_wf.m │ ├── IIR_wf.m │ ├── img │ │ └── wiener_filter.png │ ├── main.m │ └── pridict_IIR_wf.m └── code_3 │ ├── kalman.m │ ├── kalman_Sk.m │ ├── kalman_adaptive.m │ ├── kalman_forgetting_factor.m │ ├── kalman_gainfix.m │ ├── kalman_restrain_K.m │ ├── kalman_sqrt.m │ ├── kalman_sqrt1.m │ ├── main.m │ ├── main_diffuse.m │ ├── main_restrain_diffuse.m │ └── main_sqrt.m ├── 2 ├── code_0 │ ├── EKF.m │ ├── EKF_standard.m │ ├── KF.m │ ├── PF.m │ ├── UKF.m │ ├── UKF_example.m │ ├── compare.m │ ├── main.m │ ├── main_UKF.m │ └── main_UKF_PF.m └── code_1 │ ├── center_federated_filter.m │ └── federated_filter.m ├── 3 ├── code_0 │ ├── BP_standard.m │ └── Copy_of_BP_standard.m ├── code_1 │ ├── BP.m │ ├── BP_RBF.m │ ├── RBF.m │ └── example.m └── code_2 │ ├── BP.m │ ├── EKF_standard.m │ ├── PF.m │ ├── UKF.m │ └── main_UKF_PF.m ├── 4 └── code │ ├── TS_model.m │ ├── Untitled.m │ ├── fuzzy.m │ └── untitled2.m ├── LICENSE └── README.md /1/code_1/LSM.m: -------------------------------------------------------------------------------- 1 | function [X_hat,MSE]=LSM(H,Z,R) 2 | X_hat=(H'*H)^(-1)*H'*Z; 3 | MSE=(H'*H)^(-1)*H'*R*H*(H'*H)^(-1);%均方误差 4 | end 5 | 6 | -------------------------------------------------------------------------------- /1/code_1/MVE.m: -------------------------------------------------------------------------------- 1 | function [X_hat,P]=MVE(H,Z,mx,Cx,Cz) 2 | X_hat=mx+Cx*H'*(H*Cx*H'+Cz)^(-1)*(Z-H*mx); 3 | P=(Cx^(-1)+H'*Cz^(-1)*H)^(-1); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /1/code_1/WLSM.m: -------------------------------------------------------------------------------- 1 | function [X_hat,MSE]=WLSM(H,Z,R,W) 2 | X_hat=(H'*W*H)^(-1)*H'*W*Z; 3 | MSE=(H'*W*H)^(-1)*H'*W*R*W*H*(H'*W*H)^(-1);%均方误差 4 | end 5 | 6 | -------------------------------------------------------------------------------- /1/code_1/main123.m: -------------------------------------------------------------------------------- 1 | %%main 2 | clear;clc; 3 | %%状态值x误差满足均值为5，方差为0.01的正态分布 4 | %%仪器1测量误差满足均值为0，方差为0.1的正态分布 5 | %%仪器2测量误差满足均值为0，方差为0.4的正态分布 6 | x=5; %状态值x均值 7 | var0=0.1; %状态方差var0 8 | var1=0.1; %测量方差var1 9 | var2=0.4; %测量方差var2 10 | 11 | r=20; %观测次数，增大观测次数 12 | s = rng; 13 | z1 = x+sqrt(var0)*randn(1,r)+sqrt(var1)*randn(1,r); %观测值z1 14 | z2 = x+sqrt(var0)*randn(1,r)+sqrt(var2)*randn(1,r); %观测值z1 15 | z=[z1';z2'];%观测矩阵z 16 | h=ones(1,2*r)';%测量矩阵h 17 | 18 | v1=ones(1,r)*var1; 19 | v2=ones(1,r)*var2; 20 | R=diag([v1,v2]); %观测方差矩阵R 21 | 22 | [X_hat1,MSE1]=LSM(h,z,R);%最小二乘法估计 23 | [X_hat2,MSE2]=WLSM(h,z,R,R^(-1));%加权最小二乘法估计 R^(-1)为最优权重 24 | [X_hat3,MSE3]=MVE(h,z,x,var0,R);% 线性最小方差估计 25 | -------------------------------------------------------------------------------- /1/code_2/FIR_wf.m: -------------------------------------------------------------------------------- 1 | function Wopt=FIR_wf(x,d,N) 2 | % 观测信号自相关 3 | [C, lags] = xcorr(x, N, 'biased'); 4 | % 自相关矩阵R_xx，N 阶滤波器 5 | R_xx = toeplitz( C(N + 1 : end) ); 6 | % x,d 互相关函数R_xd 7 | R_xd = xcorr(d, x, N, 'biased'); 8 | R_xd = R_xd(N + 1 : end); 9 | % 维纳-霍夫方程 10 | Wopt = (R_xx)^(-1) * R_xd'; 11 | end -------------------------------------------------------------------------------- /1/code_2/IIR_wf.m: -------------------------------------------------------------------------------- 1 | 2 | function Hopt=IIR_wf(x,d,n) 3 | %%% 维纳滤波 4 | Rxx=xcorr(x); %自相关 5 | Gxx=fft(Rxx,n); %自相关功率谱密度 6 | Rxy=xcorr(x,d); %互相关 7 | Gxs=fft(Rxy,n); %互相关功率谱密度 8 | Hopt=Gxs./Gxx; %维纳霍夫方程 满足最小均方误差的滤波因子H 9 | end 10 | 11 | % Ps=fft(y); %原始信号的功率谱密度 12 | % S=H.*Ps; %经过满足最小均方误差滤波 13 | % ss=ifft(S); %信号还原 14 | -------------------------------------------------------------------------------- /1/code_2/img/wiener_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChangjingLiu/Data_Fusion_Course/d310f2b12b8dd9cb2fb0e7deaa8278fe40661e06/1/code_2/img/wiener_filter.png -------------------------------------------------------------------------------- /1/code_2/main.m: -------------------------------------------------------------------------------- 1 | clc;clear; 2 | %% 信号产生 3 | % 观测点数 4 | N = 2000; 5 | n = linspace(0, 1200, N); 6 | % 信号 7 | d = 10 * sin(pi * n / 128 + pi / 3); 8 | % 噪声（方差1.25） 9 | v = sqrt(1.25) * randn(N , 1); 10 | % 观测样本值 11 | x = d' + v; 12 | %%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | 15 | %构造FIR维纳滤波器 16 | Wopt=FIR_wf(x,d,N); %FIR维纳滤波 17 | H=IIR_wf(x,d',N); %IIR维纳滤波 18 | Ps=fft(x); %原始信号的功率谱密度 19 | S=H.*Ps; %经过满足最小均方误差滤波 20 | ss=ifft(S); %信号还原 21 | 22 | y = filter(Wopt, 1, x); 23 | % 误差 24 | En1 = d - y'; 25 | En2 = d - ss'; 26 | MSE1=mean(En1.^2); 27 | MSE2=mean(En2.^2); 28 | % 结果 29 | figure, plot(n, d, 'r:', n, y, 'b-'); 30 | legend('FIR维纳滤波信号真值','FIR维纳滤波估计值'); title('FIR维纳滤波期望信号与滤波结果对比'); 31 | xlabel('观测点数');ylabel('信号幅度'); 32 | figure, plot(n , En1); 33 | title('FIR维纳滤波误差曲线'); 34 | xlabel('观测点数');ylabel('误差幅度'); 35 | 36 | figure, plot(n, d, 'g:', n, ss, 'b-'); 37 | legend('IIR维纳滤波信号真值','IIR维纳滤波估计值'); title('IIR维纳滤波期望信号与滤波结果对比'); 38 | xlabel('观测点数');ylabel('信号幅度'); 39 | figure, plot(n , En2); 40 | title('IIR维纳滤波误差曲线'); 41 | xlabel('观测点数');ylabel('误差幅度'); -------------------------------------------------------------------------------- /1/code_2/pridict_IIR_wf.m: -------------------------------------------------------------------------------- 1 | 2 | function Hopt=IIR_wf(x,d,n) 3 | %%% 维纳滤波 4 | Rxx=xcorr(x); %自相关 5 | Gxx=fft(Rxx,n); %自相关功率谱密度 6 | Rxy=xcorr(x,d); %互相关 7 | Gxs=fft(Rxy,n); %互相关功率谱密度 8 | Hopt=Gxs./Gxx; %维纳霍夫方程 满足最小均方误差的滤波因子H 9 | end 10 | 11 | % Ps=fft(y); %原始信号的功率谱密度 12 | % S=H.*Ps; %经过满足最小均方误差滤波 13 | % ss=ifft(S); %信号还原 14 | -------------------------------------------------------------------------------- /1/code_3/kalman.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 基本离散kalman滤波 3 | for i=2:N 4 | Xn=phi*Xkf(:,i-1)+Tau*u;%预测 5 | P1=phi*P0*phi'+Q;%预测误差协方差 6 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 7 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 8 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 9 | end 10 | end -------------------------------------------------------------------------------- /1/code_3/kalman_Sk.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_Sk(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 扩大P(k|k-1)的kalman滤波(Sk法) 3 | for i=2:N 4 | Xn=phi*Xkf(:,i-1)+Tau*u;%预测 5 | e=Z(:,i)-H*Xn; %新息 6 | E=H*P0*H'+R; %量测估计误差 7 | r=3; 8 | if(e'*e>r*trace(E)) 9 | disp("发散"); 10 | Sk=trace(e*e'-H*Q*H'-R)/trace(H*phi*P0*phi'*H'); 11 | P1=Sk*phi*P0*phi'+Q;%预测误差协方差 12 | else 13 | P1=phi*P0*phi'+Q;%预测误差协方差 14 | end 15 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 16 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 17 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 18 | end 19 | end -------------------------------------------------------------------------------- /1/code_3/kalman_adaptive.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_adaptive(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 自适应kalman滤波 3 | K=[1;0]; 4 | lambda=0.95;%遗忘因子 5 | for i=2:N 6 | Xn=phi*Xkf(:,i-1)+Tau*u; %预测 7 | P1=phi*P0*phi'+Q; %预测误差协方差 8 | dk=(1-lambda)*(1-lambda^(i)); 9 | vk=Z(:,i)-H*Xn; 10 | R=(1-dk)*R+dk*((eye(1)-H*K)*vk*vk'*(eye(1)-H*K)'+H*P0*H'); %观测误差R基于dk调节 11 | K=P1*H'*(H*P1*H'+R)^(-1); %增益 12 | Xkf(:,i)=Xn+K*vk;%状态更新 13 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 14 | end 15 | end -------------------------------------------------------------------------------- /1/code_3/kalman_forgetting_factor.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_forgetting_factor(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 带遗忘因子的kalman滤波 3 | lambda=0.5; 4 | for i=2:N 5 | Xn=phi*Xkf(:,i-1)+Tau*u; %预测 6 | P1=phi*P0*phi'+Q; %预测误差协方差 7 | K=P1*H'*(H*P1*H'+R)^(-1); %增益 8 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn); %状态更新 9 | P0=lambda^(-1)*(eye(2)-K*H)*P1; %滤波误差协方差更新 10 | end 11 | end -------------------------------------------------------------------------------- /1/code_3/kalman_gainfix.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_gainfix(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 常值增益kalman滤波 3 | K=[0.5;0.1]; 4 | for i=2:N 5 | Xn=phi*Xkf(:,i-1)+Tau*u;%预测 6 | P1=phi*P0*phi'+Q;%预测误差协方差 7 | % K=P1*H'*(H*P1*H'+R)^(-1);%增益 8 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 9 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 10 | end -------------------------------------------------------------------------------- /1/code_3/kalman_restrain_K.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_restrain_K(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 限制k的kalman滤波 3 | K=[0;0]; 4 | for i=2:N 5 | Xn=phi*Xkf(:,i-1)+Tau*u;%预测 6 | P1=phi*P0*phi'+Q;%预测误差协方差 7 | e=Z(:,i)-H*Xn; %新息 8 | E=H*P0*H'+R; %量测估计误差 9 | r=3; 10 | if(e'*e>r*trace(E)) 11 | disp("发散"); 12 | %重置k 13 | % K=[0.5;0.1]; 14 | if(i==2) 15 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 16 | end 17 | else 18 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 19 | end 20 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 21 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 22 | end 23 | end -------------------------------------------------------------------------------- /1/code_3/kalman_sqrt.m: -------------------------------------------------------------------------------- 1 | function Xkf=kalman_sqrt(Xkf,u,Z,H,P0,Q,R,Phi,Tau,N) 2 | sQ = mychol(Q); sR = mychol(R); Delta0 = mychol(P0); 3 | %% 平方根kalman滤波 4 | for i=2:N 5 | %状态更新 6 | Xn=Phi*Xkf(:,i-1)+Tau*u;%预测 7 | 8 | %预测协方差更新 9 | [~, Delta] = myqr([Phi*Delta0, Tau*sQ]'); %[Phi*Delta0, Tau*sQ]'系统驱动噪声 10 | Delta = Delta';%delta_k+1/k=U^T上三角阵,这里已经完成了计算 11 | 12 | [~, rho] = myqr([H*Delta, sR]'); 13 | rho = rho';%相当于 14 | 15 | %测量更新 16 | K=Delta*Delta'*H'*(rho*rho')^(-1); 17 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 18 | Delta0 = Delta*(eye(length(Delta0))-Delta'*H'*(rho*rho'+sR*rho')^-1*H*Delta); 19 | end 20 | end 21 | 22 | function [Phi, Tau, H, Q, R, P0] = rndmodel(n, m, l) % 随机系统模型 23 | Phi = randn(n); Tau = randn(n,l); H = randn(m,n); 24 | Q = diag(randn(l,1))^2; R = diag(randn(m,1))^2; 25 | P0 = randn(n); P0 = P0'*P0; 26 | end 27 | 28 | function Delta1 = SRKF(Delta0, Phi, Tau, sQ, H, sR) % 平方根滤波(核心时不断更新协方差的平方根) 29 | %Delta0：协方差P的平方根 30 | %phi： 31 | %tau： 32 | %sq：Q平方根 33 | %H：H测量矩阵 34 | %sr：R平方根 35 | %%预测 36 | [q, Delta] = myqr([Phi*Delta0, Tau*sQ]'); %[Phi*Delta0, Tau*sQ]'系统驱动噪声 37 | Delta = Delta';%delta_k+1/k=U^T上三角阵,这里已经完成了计算 38 | 39 | [q, rho] = myqr([H*Delta, sR]'); 40 | rho = rho';%相当于 41 | Delta1 = Delta*(eye(length(Delta0))-Delta'*H'*(rho*rho'+sR*rho')^-1*H*Delta); 42 | end 43 | 44 | function A = mychol(P) % 乔莱斯基分解，P=A*A', A为上三角阵 45 | n = length(P); A = zeros(n); 46 | for j=n:-1:1 47 | A(j,j) = sqrt(P(j,j)-A(j,j+1:n)*A(j,j+1:n)'); 48 | for i=(j-1):-1:1 49 | A(i,j) = (P(i,j)-A(i,j+1:n)*A(j,j+1:n)')/A(j,j); 50 | end 51 | end 52 | end 53 | 54 | function [Q, R] = myqr(A) % QR分解，A=Q*R, 其中Q'*Q=I，R为上三角阵 55 | [m, n] = size(A); 56 | if n>m, error('n must not less than m.'); end 57 | R = zeros(n); 58 | for i=1:n 59 | R(i,i) = sqrt(A(:,i)'*A(:,i)); 60 | A(:,i) = A(:,i)/R(i,i); 61 | j = i+1:n; 62 | R(i,j) = A(:,i)'*A(:,j); 63 | A(:,j) = A(:,j)-A(:,i)*R(i,j); 64 | end 65 | Q = A; 66 | 67 | end 68 | -------------------------------------------------------------------------------- /1/code_3/kalman_sqrt1.m: -------------------------------------------------------------------------------- 1 | function test_srkf 2 | % Copyright(c) 2009-2016, by Gongmin Yan, All rights reserved. 3 | % Northwestern Polytechnical University, Xi An, P.R.China 4 | % 12/11/2016 5 | n = 10; m = 3; l = 2; 6 | [Phi, Tau, H, Q, R, P0] = rndmodel(n, m, l); 7 | % (1) 标准KF 8 | P10 = Phi*P0*Phi'+Tau*Q*Tau';%协方差 9 | P1 = P10 - P10*H'*(H*P10*H'+R)^-1*H*P10;%更新协方差 10 | % (2) 平方根KF 11 | sQ = mychol(Q); sR = mychol(R); Delta0 = mychol(P0); 12 | Delta1 = SRKF(Delta0, Phi, Tau, sQ, H, sR); 13 | errSRKF = P1 - Delta1*Delta1', 14 | end 15 | 16 | function [Phi, Tau, H, Q, R, P0] = rndmodel(n, m, l) % 随机系统模型 17 | Phi = randn(n); Tau = randn(n,l); H = randn(m,n); 18 | Q = diag(randn(l,1))^2; R = diag(randn(m,1))^2; 19 | P0 = randn(n); P0 = P0'*P0; 20 | end 21 | 22 | function Delta1 = SRKF(Delta0, Phi, Tau, sQ, H, sR) % 平方根滤波(核心时不断更新协方差的平方根) 23 | %Delta0：协方差P的平方根 24 | %phi： 25 | %tau： 26 | %sq：Q平方根 27 | %H：H测量矩阵 28 | %sr：R平方根 29 | %%预测 30 | [q, Delta] = myqr([Phi*Delta0, Tau*sQ]'); %[Phi*Delta0, Tau*sQ]'系统驱动噪声 31 | Delta = Delta';%delta_k+1/k=U^T上三角阵,这里已经完成了计算 32 | 33 | [q, rho] = myqr([H*Delta, sR]'); 34 | rho = rho';%相当于 35 | Delta1 = Delta*(eye(length(Delta0))-Delta'*H'*(rho*rho'+sR*rho')^-1*H*Delta); 36 | end 37 | 38 | function A = mychol(P) % 乔莱斯基分解，P=A*A', A为上三角阵 39 | n = length(P); A = zeros(n); 40 | for j=n:-1:1 41 | A(j,j) = sqrt(P(j,j)-A(j,j+1:n)*A(j,j+1:n)'); 42 | for i=(j-1):-1:1 43 | A(i,j) = (P(i,j)-A(i,j+1:n)*A(j,j+1:n)')/A(j,j); 44 | end 45 | end 46 | end 47 | 48 | function [Q, R] = myqr(A) % QR分解，A=Q*R, 其中Q'*Q=I，R为上三角阵 49 | [m, n] = size(A); 50 | if n>m, error('n must not less than m.'); end 51 | R = zeros(n); 52 | for i=1:n 53 | R(i,i) = sqrt(A(:,i)'*A(:,i)); 54 | A(:,i) = A(:,i)/R(i,i); 55 | j = i+1:n; 56 | R(i,j) = A(:,i)'*A(:,j); 57 | A(:,j) = A(:,j)-A(:,i)*R(i,j); 58 | end 59 | Q = A; 60 | 61 | end 62 | -------------------------------------------------------------------------------- /1/code_3/main.m: -------------------------------------------------------------------------------- 1 | 2 | %% 匀速直线运动 3 | clc;clear; 4 | close all; 5 | T=1;%雷达扫描周期 6 | N=25/T;%总的采样次数 7 | X=zeros(2,N);%目标真实位置、速度 8 | X(:,1)=[0,2];%目标初始位置、速度 9 | S(:,1)=[0,2]; 10 | Z=zeros(1,N);%传感器对位置的观测 11 | Z(:,1)=X(1,1);%观测初始化 12 | delta_w=1e-1; %如果增大这个参数，目标真实轨迹就是曲线了 13 | 14 | Q=delta_w*diag([2,2]);%过程噪声方差 15 | R=eye(1)*10;%观测噪声均值 16 | 17 | phi=[1,T;0,1];%状态转移矩阵 18 | Tau=[T^2/2 0;0,T]; 19 | u=[1;1]; 20 | W=[1;0]; 21 | H=[1,0];%观测矩阵 22 | Xn=zeros(2,0); 23 | Xn_fix=zeros(2,0); 24 | 25 | for i=2:N 26 | S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹 27 | X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 28 | Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测 29 | end 30 | 31 | % Kalman 滤波 32 | Xkf=zeros(2,N); 33 | Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 34 | Xkf_gainfix=Xkf; 35 | P0=100e-2*eye(2);%协方差阵初始化 36 | %% 基本离散kalman滤波 37 | tic; 38 | Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N); 39 | toc; 40 | 41 | %% 固定增益的kalman滤波 42 | tic; 43 | Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N); 44 | toc; 45 | 46 | %误差分析 47 | for i=1:N 48 | 49 | Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 50 | Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差 51 | Err_fixKalmanFilter(i)=RMS(X(:,i),Xkf_gainfix(:,i));%滤波后的误差 52 | end 53 | mean_Observation=mean(Err_Observation); 54 | mean_KalmanFilter=mean(Err_KalmanFilter); 55 | mean_fixKalmanFilter=mean(Err_fixKalmanFilter); 56 | figure 57 | hold on;box on; 58 | t=(0:1:N-1); 59 | plot(t,S(1,:),'g','LineWidth',1);%理论轨迹 60 | plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 61 | plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 62 | plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 63 | plot(t,Xkf_gainfix(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 64 | % M=M'; 65 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 66 | legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','固定增益滤波后轨迹'); 67 | xlabel('横坐标 T/s'); 68 | ylabel('纵坐标 X/m'); 69 | 70 | figure 71 | hold on;box on; 72 | plot(t,Err_Observation,'-'); 73 | plot(t,Err_KalmanFilter,'--'); 74 | plot(t,Err_fixKalmanFilter,'-.'); 75 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 76 | legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),sprintf('固定增益滤波后误差%.03f',mean_fixKalmanFilter)); 77 | xlabel('观测时间/s'); 78 | ylabel('误差值'); 79 | 80 | 81 | % 计算欧氏距离子函数 82 | function dist=RMS(X1,X2) 83 | if length(X2)<=2 84 | dist=sqrt((X1(1)-X2(1))^2); 85 | else 86 | dist=sqrt((X1(1)-X2(1))^2); 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /1/code_3/main_diffuse.m: -------------------------------------------------------------------------------- 1 | 2 | %% 匀速直线运动 3 | clc;clear; 4 | close all; 5 | T=1;%雷达扫描周期 6 | N=25/T;%总的采样次数 7 | X=zeros(2,N);%目标真实位置、速度 8 | X(:,1)=[0,2];%目标初始位置、速度 9 | S(:,1)=[0,2]; 10 | Z=zeros(1,N);%传感器对位置的观测 11 | Z(:,1)=X(1,1);%观测初始化 12 | delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了 13 | 14 | Q=delta_w*diag([1,1.5]);%过程噪声方差 15 | R=eye(1);%观测噪声均值 16 | 17 | phi=[1,T;0,1];%状态转移矩阵 18 | phi_1=[1,0;0,1]; 19 | Tau=[T^2/2 0;0,T]; 20 | u=[0;0]; 21 | W=[1;0]; 22 | H=[1,0];%观测矩阵 23 | Xn=zeros(2,0); 24 | Xn_fix=zeros(2,0); 25 | 26 | for i=2:N 27 | S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹 28 | % X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 29 | X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 30 | Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测 31 | end 32 | 33 | % Kalman 滤波 34 | Xkf=zeros(2,N); 35 | Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 36 | Xkf_gainfix=Xkf; 37 | P0=100e-2*eye(2);%协方差阵初始化 38 | a=0; 39 | ee=zeros(1,N); 40 | EE=zeros(1,N); 41 | %% 基本离散kalman滤波 42 | for i=2:N 43 | % Xn=phi*Xkf(:,i-1)+Tau*u;%预测 44 | 45 | Xn=phi_1*Xkf(:,i-1)+Tau*u;%假设预测错误 46 | P1=phi_1*P0*phi_1'+Q;%预测误差协方差 47 | e=Z(:,i)-H*Xn; %新息 48 | E=H*P0*H'+R; %量测估计误差 49 | % 判断是否发散 50 | r=1;%储备系数，一般取1 51 | ee(:,i)=e'*e; 52 | EE(:,i)=r*trace(E); 53 | if(e'*e>r*trace(E)) 54 | disp("发散"); 55 | end 56 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 57 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 58 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 59 | end 60 | 61 | %% 固定增益的kalman滤波 62 | % Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N); 63 | 64 | %误差分析 65 | for i=1:N 66 | 67 | Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 68 | Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差 69 | % Err_fixKalmanFilter(i)=RMS(X(:,i),Xkf_gainfix(:,i));%滤波后的误差 70 | end 71 | mean_Observation=mean(Err_Observation); 72 | mean_KalmanFilter=mean(Err_KalmanFilter); 73 | % mean_fixKalmanFilter=mean(Err_fixKalmanFilter); 74 | figure 75 | hold on;box on; 76 | t=(0:1:N-1); 77 | plot(t,S(1,:),'g','LineWidth',1);%理论轨迹 78 | plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 79 | plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 80 | plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 81 | % plot(t,Xkf_gainfix(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 82 | % M=M'; 83 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 84 | legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹'); 85 | xlabel('横坐标 T/s'); 86 | ylabel('纵坐标 X/m'); 87 | 88 | figure 89 | hold on;box on; 90 | plot(t,Err_Observation,'-'); 91 | plot(t,Err_KalmanFilter,'--'); 92 | % plot(t,Err_fixKalmanFilter,'-.'); 93 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 94 | legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter)); 95 | xlabel('观测时间/s'); 96 | ylabel('误差值'); 97 | 98 | figure 99 | hold on;box on; 100 | plot(t,ee,'-'); 101 | plot(t,EE,'--'); 102 | legend("判据左侧","判据右侧"); 103 | xlabel('观测时间/s'); 104 | ylabel('误差值'); 105 | 106 | 107 | % 计算欧氏距离子函数 108 | function dist=RMS(X1,X2) 109 | if length(X2)<=2 110 | dist=sqrt((X1(1)-X2(1))^2); 111 | else 112 | dist=sqrt((X1(1)-X2(1))^2); 113 | end 114 | end 115 | -------------------------------------------------------------------------------- /1/code_3/main_restrain_diffuse.m: -------------------------------------------------------------------------------- 1 | 2 | %% 匀速直线运动 3 | clc;clear; 4 | close all; 5 | T=1;%雷达扫描周期 6 | N=50/T;%总的采样次数 7 | X=zeros(2,N);%目标真实位置、速度 8 | X(:,1)=[5,0];%目标初始位置、速度 9 | S(:,1)=[5,0]; 10 | Z=zeros(1,N);%传感器对位置的观测 11 | Z(:,1)=X(1,1);%观测初始化 12 | delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了 13 | 14 | Q=delta_w*diag([1,0]);%过程噪声方差 15 | R=eye(1)*0.1;%观测噪声均值 16 | 17 | phi=[0.5,0;0,0];%状态转移矩阵 18 | phi_1=[0.5,0;0,0]; 19 | Tau=[T^2/2 0;0,T]; 20 | u=[8;0];%加速度矩阵 21 | u_1=[0;0]; 22 | W=[1;0]; 23 | H=[1,0];%观测矩阵 24 | Xn=zeros(2,0); 25 | Xn_fix=zeros(2,0); 26 | 27 | for i=2:N 28 | S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹 29 | X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 30 | Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测 31 | end 32 | 33 | % Kalman 滤波 34 | Xkf=zeros(2,N); 35 | Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 36 | Xkf_rk=Xkf; 37 | Xkf_ff=Xkf; 38 | Xkf_Sk=Xkf; 39 | P0=100e-2*eye(2);%协方差阵初始化 40 | %% 基本离散kalman滤波 41 | Xkf=kalman(Xkf,u_1,Z,H,P0,Q,R,phi,Tau,N); 42 | 43 | %% 限制K减小的kalman滤波 44 | Xkf_rk=kalman_restrain_K(Xkf_rk,u_1,Z,H,P0,Q,R,phi,Tau,N); 45 | 46 | %% 带遗忘因子的kalman滤波 47 | Xkf_ff=kalman_forgetting_factor(Xkf_ff,u_1,Z,H,P0,Q,R,phi,Tau,N); 48 | 49 | %% 扩大P的kalman滤波 50 | Xkf_Sk=kalman_Sk(Xkf_Sk,u_1,Z,H,P0,Q,R,phi,Tau,N); 51 | 52 | %误差分析 53 | for i=1:N 54 | 55 | Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 56 | Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差 57 | Err_rkKalmanFilter(i)=RMS(X(:,i),Xkf_rk(:,i));%滤波后的误差 58 | Err_ffKalmanFilter(i)=RMS(X(:,i),Xkf_ff(:,i));%滤波后的误差 59 | Err_adKalmanFilter(i)=RMS(X(:,i),Xkf_Sk(:,i));%滤波后的误差 60 | end 61 | mean_Observation=mean(Err_Observation); 62 | mean_KalmanFilter=mean(Err_KalmanFilter); 63 | mean_fixKalmanFilter=mean(Err_rkKalmanFilter); 64 | mean_ffKalmanFilter=mean(Err_ffKalmanFilter); 65 | mean_adKalmanFilter=mean(Err_adKalmanFilter); 66 | 67 | %% 画图 68 | figure 69 | hold on;box on; 70 | t=(0:1:N-1); 71 | plot(t,S(1,:),'-','LineWidth',1);%理论轨迹 72 | plot(t,X(1,:),'--','LineWidth',1);%实际轨迹 73 | plot(t,Z(1,:),'-o','LineWidth',1);%观测轨迹 74 | plot(t,Xkf(1,:),':','LineWidth',2);%卡尔曼滤波轨迹 75 | plot(t,Xkf_rk(1,:),'-.','LineWidth',2);%卡尔曼滤波轨迹 76 | plot(t,Xkf_ff(1,:),'-x','LineWidth',2);%卡尔曼滤波轨迹 77 | plot(t,Xkf_Sk(1,:),'--s','LineWidth',2);%卡尔曼滤波轨迹 78 | % M=M'; 79 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 80 | legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','限制k减小滤波后轨迹','带遗忘因子卡尔曼滤波','扩大P的卡尔曼滤波'); 81 | xlabel('横坐标 T/s'); 82 | ylabel('纵坐标 X/m'); 83 | 84 | figure 85 | hold on;box on; 86 | plot(t,Err_Observation,'-'); 87 | plot(t,Err_KalmanFilter,'--'); 88 | plot(t,Err_rkKalmanFilter,'-.'); 89 | plot(t,Err_ffKalmanFilter,'-x'); 90 | plot(t,Err_adKalmanFilter,'-s'); 91 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 92 | legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),... 93 | sprintf('限制k减小增益滤波后误差%.03f',mean_fixKalmanFilter),sprintf('带遗忘因子滤波后误差%.03f',mean_ffKalmanFilter),... 94 | sprintf('扩大P的滤波后误差%.03f',mean_adKalmanFilter)); 95 | xlabel('观测时间/s'); 96 | ylabel('误差值'); 97 | 98 | 99 | % 计算欧氏距离子函数 100 | function dist=RMS(X1,X2) 101 | if length(X2)<=2 102 | dist=sqrt((X1(1)-X2(1))^2); 103 | else 104 | dist=sqrt((X1(1)-X2(1))^2); 105 | end 106 | end 107 | -------------------------------------------------------------------------------- /1/code_3/main_sqrt.m: -------------------------------------------------------------------------------- 1 | %% 匀速直线运动 2 | clc;clear; 3 | close all; 4 | T=1;%雷达扫描周期 5 | N=25/T;%总的采样次数 6 | X=zeros(2,N);%目标真实位置、速度 7 | X(:,1)=[0,2];%目标初始位置、速度 8 | S(:,1)=[0,2]; 9 | Z=zeros(1,N);%传感器对位置的观测 10 | Z(:,1)=X(1,1);%观测初始化 11 | delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了 12 | 13 | Q=delta_w*diag([1,1.5]);%过程噪声方差 14 | R=eye(1);%观测噪声均值 15 | 16 | phi=[1000000,T;0,1000000];%状态转移矩阵%%%%%% 此处修改 17 | Tau=[T^2/2,0;0,T]; 18 | u=[0;0]; 19 | W=[1;0]; 20 | H=[1,0];%观测矩阵 21 | Xn=zeros(2,0); 22 | Xn_fix=zeros(2,0); 23 | 24 | for i=2:N 25 | S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹 26 | X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 27 | Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测 28 | end 29 | 30 | % Kalman 滤波 31 | Xkf=zeros(2,N); 32 | Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 33 | Xkf_gainfix=Xkf; 34 | Xkf_sqrt=Xkf; 35 | P0=100e-2*eye(2);%协方差阵初始化 36 | %% 基本离散kalman滤波 37 | Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N); 38 | 39 | %% 固定增益的kalman滤波 40 | % Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N); 41 | 42 | Xkf_sqrt=kalman_sqrt(Xkf_sqrt,u,Z,H,P0,Q,R,phi,Tau,N); 43 | 44 | %误差分析 45 | for i=1:N 46 | 47 | Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 48 | Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差 49 | Err_sqrtKalmanFilter(i)=RMS(X(:,i),Xkf_sqrt(:,i));%滤波后的误差 50 | end 51 | mean_Observation=mean(Err_Observation); 52 | mean_KalmanFilter=mean(Err_KalmanFilter); 53 | mean_fixKalmanFilter=mean(Err_sqrtKalmanFilter); 54 | figure 55 | hold on;box on; 56 | t=(0:1:N-1); 57 | plot(t,S(1,:),'g','LineWidth',1);%理论轨迹 58 | plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 59 | plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 60 | plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 61 | plot(t,Xkf_sqrt(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 62 | % M=M'; 63 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 64 | legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','平方根滤波后轨迹'); 65 | xlabel('横坐标 T/s'); 66 | ylabel('纵坐标 X/m'); 67 | 68 | figure 69 | hold on;box on; 70 | plot(t,Err_Observation,'-'); 71 | plot(t,Err_KalmanFilter,'--'); 72 | plot(t,Err_sqrtKalmanFilter,'-.'); 73 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 74 | legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),sprintf('平方根滤波后误差%.03f',mean_fixKalmanFilter)); 75 | xlabel('观测时间/s'); 76 | ylabel('误差值'); 77 | 78 | 79 | % 计算欧氏距离子函数 80 | function dist=RMS(X1,X2) 81 | if length(X2)<=2 82 | dist=sqrt((X1(1)-X2(1))^2); 83 | else 84 | dist=sqrt((X1(1)-X2(1))^2); 85 | end 86 | end 87 | -------------------------------------------------------------------------------- /2/code_0/EKF.m: -------------------------------------------------------------------------------- 1 | clear;clc; close all; 2 | N = 50; 3 | Q = 10;w=sqrt(Q)*randn(1,N); 4 | R = 1;v=sqrt(R)*randn(1,N); 5 | P =eye(1); 6 | x=zeros(1,N); 7 | delta_x=zeros(1,N); 8 | s=zeros(1,N); 9 | Xekf=zeros(1,N); 10 | Xekf1=zeros(1,N); 11 | x(1,1)=0.1; 12 | delta_x(1,1)=0; 13 | s(1,1)=0.1; 14 | Xekf(1,1)=x(1,1); 15 | Xekf1(1,1)=x(1,1); 16 | z=zeros(1,N); 17 | delta_z=zeros(1,N); 18 | z(1)=x(1,1)^2/20+v(1); 19 | delta_z(1)=x(1,1)/10+v(1); 20 | zpre=zeros(1,N); 21 | zpre(1,1)=z(1); 22 | 23 | for k = 2 : N 24 | % 模拟系统 25 | s(:,k) = 0.5 * s(:,k-1) + (2.5 * s(:,k-1) / (1 + s(:,k-1).^2)) + 8 * cos(1.2*(k-1)); 26 | x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1); 27 | delta_x(:,k)=(0.5 + 2.5 * (1-x(:,k-1).^2)/((1+x(:,k-1).^2).^2))*delta_x(:,k-1)+w(k-1); 28 | z(k) = x(:,k).^2 / 20 + v(k); 29 | delta_z(k)=x(:,k)/10*delta_x(:,k)+v(1); 30 | end 31 | 32 | %按标称状态线性化的卡尔曼滤波 33 | for k=2:N 34 | %状态预测(不变) 35 | Xpre = 0.5*Xekf1(:,k-1)+ 2.5*Xekf1(:,k-1)/(1+Xekf1(:,k-1).^2) + 8 * cos(1.2*(k-1));%f 36 | 37 | F = 0.5 + 2.5 * (1-Xekf1.^2)/((1+Xekf1.^2).^2);%f_n 38 | H = Xpre/10; 39 | 40 | %均方差预测方程 41 | PP=F*P*F'+Q; 42 | 43 | %卡尔曼增益（不变） 44 | Kk=PP*H'*(H*PP*H'+R)^(-1); 45 | 46 | 47 | delta_x(:,k)=F*delta_x(:,k-1)+Kk*(delta_z(:,k)-H*delta_x(:,k-1)); 48 | 49 | %状态更新 50 | Xekf1(k)=Xpre+delta_x(k); 51 | 52 | %均方差更新 53 | P=PP-Kk*H*PP; 54 | end 55 | % 广义卡尔曼滤波 56 | for k = 2 : N 57 | %状态预测 58 | Xpre = 0.5*Xekf(:,k-1)+ 2.5*Xekf(:,k-1)/(1+Xekf(:,k-1).^2) + 8 * cos(1.2*(k-1)); 59 | 60 | F = 0.5 + 2.5 * (1-Xekf.^2)/((1+Xekf.^2).^2); 61 | H = Xpre/10; 62 | 63 | %均方差预测方程 64 | PP=F*P*F'+Q; 65 | 66 | %卡尔曼增益 67 | Kk=PP*H'*(H*PP*H'+R)^(-1); 68 | 69 | %状态更新 70 | zpre =Xpre.^2/20; 71 | Xekf(k)=Xpre+Kk*(z(k)-zpre); 72 | 73 | %均方差更新 74 | P=PP-Kk*H*PP; 75 | end 76 | 77 | %误差分析 78 | for i=1:N 79 | Err_Obs(i)=RMS(x(:,i),z(:,i));%滤波前的误差 80 | Err_EKF(i)=RMS(x(:,i),Xekf(:,i));%滤波后的误差 81 | Err_EKF1(i)=RMS(x(:,i),Xekf1(:,i));%滤波后的误差 82 | end 83 | mean_Obs=mean(Err_Obs); 84 | mean_EKF=mean(Err_EKF); 85 | mean_EKF1=mean(Err_EKF1); 86 | % t = 2 : N; 87 | % figure; 88 | % plot(t,x(1,t),'b',t,Xekf(1,t),'r*'); 89 | % legend('真实值','EKF估计值'); 90 | figure 91 | hold on;box on; 92 | t=(0:1:N-1); 93 | plot(t,s(1,:),'g','LineWidth',1);%理论轨迹 94 | plot(t,x(1,:),'--b','LineWidth',1);%实际轨迹 95 | plot(t,z(1,:),'-or','LineWidth',1);%观测轨迹 96 | plot(t,Xekf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 97 | plot(t,Xekf1(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 98 | % M=M'; 99 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 100 | legend('理论轨迹','实际运动轨迹','观测轨迹','扩展卡尔曼滤波后轨迹','标称状态线性化'); 101 | xlabel('横坐标 T/s'); 102 | ylabel('纵坐标 X/m'); 103 | 104 | figure 105 | hold on;box on; 106 | plot(t,Err_Obs,'-'); 107 | plot(t,Err_EKF,'--'); 108 | plot(t,Err_EKF1,'-.'); 109 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 110 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('扩展卡尔曼滤波后误差%.03f',mean_EKF),sprintf('按标称线性化滤波后误差%.03f',mean_EKF1)); 111 | xlabel('观测时间/s'); 112 | ylabel('误差值'); 113 | 114 | 115 | % 计算欧氏距离子函数 116 | function dist=RMS(X1,X2) 117 | if length(X2)<=2 118 | dist=sqrt((X1(1)-X2(1))^2); 119 | else 120 | dist=sqrt((X1(1)-X2(1))^2); 121 | end 122 | end 123 | 124 | -------------------------------------------------------------------------------- /2/code_0/EKF_standard.m: -------------------------------------------------------------------------------- 1 | clear;clc; close all; 2 | N = 50; 3 | Q = 10;w=sqrt(Q)*randn(1,N); 4 | R = 5;v=sqrt(R)*randn(1,N); 5 | P =eye(1); 6 | x=zeros(1,N); 7 | s=zeros(1,N); 8 | Xekf=zeros(1,N); 9 | x(1,1)=0.1; 10 | s(1,1)=0.1; 11 | Xekf(1,1)=x(1,1); 12 | z=zeros(1,N); 13 | % z(1)=x(1,1)^2/20+v(1); 14 | z(1)=x(1,1)+v(1); 15 | zpre=zeros(1,N); 16 | zpre(1,1)=z(1); 17 | 18 | for k = 2 : N 19 | % 模拟系统 20 | s(:,k) = 0.5 * s(:,k-1) + (2.5 * s(:,k-1) / (1 + s(:,k-1).^2)) + 8 * cos(1.2*(k-1)); 21 | x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1); 22 | 23 | % z(k) = x(:,k).^2 / 20 + v(k); 24 | z(k) = x(:,k)+ v(k); 25 | end 26 | 27 | 28 | % 广义卡尔曼滤波 29 | for k = 2 : N 30 | %状态预测 31 | Xpre = 0.5*Xekf(:,k-1)+ 2.5*Xekf(:,k-1)/(1+Xekf(:,k-1).^2) + 8 * cos(1.2*(k-1)); 32 | 33 | F = 0.5 + 2.5 * (1-Xekf.^2)/((1+Xekf.^2).^2); 34 | % H = Xpre/10; 35 | H = 1; 36 | 37 | %均方差预测方程 38 | PP=F*P*F'+Q; 39 | 40 | %卡尔曼增益 41 | Kk=PP*H'*(H*PP*H'+R)^(-1); 42 | 43 | %状态更新 44 | % zpre =Xpre.^2/20; 45 | zpre =Xpre; 46 | Xekf(k)=Xpre+Kk*(z(k)-zpre); 47 | 48 | %均方差更新 49 | P=PP-Kk*H*PP; 50 | end 51 | 52 | %误差分析 53 | for i=1:N 54 | Err_Obs(i)=RMS(x(:,i),z(:,i));%滤波前的误差 55 | Err_EKF(i)=RMS(x(:,i),Xekf(:,i));%滤波后的误差 56 | % Err_EKF1(i)=RMS(x(:,i),Xekf1(:,i));%滤波后的误差 57 | end 58 | mean_Obs=mean(Err_Obs); 59 | mean_EKF=mean(Err_EKF); 60 | % mean_EKF1=mean(Err_EKF1); 61 | % t = 2 : N; 62 | % figure; 63 | % plot(t,x(1,t),'b',t,Xekf(1,t),'r*'); 64 | % legend('真实值','EKF估计值'); 65 | figure 66 | hold on;box on; 67 | t=(0:1:N-1); 68 | plot(t,s(1,:),'g','LineWidth',1);%理论轨迹 69 | plot(t,x(1,:),'--b','LineWidth',1);%实际轨迹 70 | plot(t,z(1,:),'-or','LineWidth',1);%观测轨迹 71 | plot(t,Xekf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 72 | % plot(t,Xekf1(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 73 | % M=M'; 74 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 75 | legend('理论轨迹','实际运动轨迹','观测轨迹','扩展卡尔曼滤波后轨迹'); 76 | xlabel('横坐标 T/s'); 77 | ylabel('纵坐标 X/m'); 78 | 79 | figure 80 | hold on;box on; 81 | plot(t,Err_Obs,'-'); 82 | plot(t,Err_EKF,'--'); 83 | % plot(t,Err_EKF1,'-.'); 84 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 85 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('扩展卡尔曼滤波后误差%.03f',mean_EKF)); 86 | xlabel('观测时间/s'); 87 | ylabel('误差值'); 88 | 89 | 90 | % 计算欧氏距离子函数 91 | function dist=RMS(X1,X2) 92 | if length(X2)<=2 93 | dist=sqrt((X1(1)-X2(1))^2); 94 | else 95 | dist=sqrt((X1(1)-X2(1))^2); 96 | end 97 | end 98 | 99 | -------------------------------------------------------------------------------- /2/code_0/KF.m: -------------------------------------------------------------------------------- 1 | function Xkf=KF(Xkf,u,Z,H,P0,Q,R,phi,Tau,N) 2 | %% 基本离散kalman滤波 3 | for i=2:N 4 | Xn=phi*Xkf(:,i-1)+Tau*u;%预测 5 | P1=phi*P0*phi'+Q;%预测误差协方差 6 | K=P1*H'*(H*P1*H'+R)^(-1);%增益 7 | Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新 8 | P0=(eye(2)-K*H)*P1; %滤波误差协方差更新 9 | end 10 | end -------------------------------------------------------------------------------- /2/code_0/PF.m: -------------------------------------------------------------------------------- 1 | function Xpf=PF(Xpf,Z,Q,R,M,N) 2 | %粒子滤波初始化 3 | V = 2; %初始分布的方差 4 | x_P = []; % 粒子 5 | % 用一个高斯分布随机的产生初始的粒子 6 | for i = 1:M 7 | x_P(i) = Xpf(:,1) + sqrt(V) * randn; 8 | end 9 | 10 | %粒子滤波 11 | for t = 1:N 12 | for i = 1:M 13 | %从先验p(x(k)|x(k-1))中采样 14 | x_P_update(:,i) = gfun(x_P(:,i),t) + sqrt(Q)*randn; 15 | %计算采样粒子的值，为后面根据似然去计算权重做铺垫 16 | z_update(:,i) = x_P_update(:,i)^2/20; 17 | %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式 18 | P_w(:,i) = (1/sqrt(2*pi*R)) * exp(-(Z(:,t+1) - z_update(:,i))^2/(2*R)); 19 | end 20 | % 归一化. 21 | P_w = P_w./sum(P_w); 22 | 23 | %% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的 24 | for i = 1 : M 25 | x_P(:,i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代 26 | end % find( ,1) 返回第一个 符合前面条件的数的 下标 27 | 28 | %状态估计，重采样以后，每个粒子的权重都变成了1/N 29 | Xpf(:,t+1)=mean(x_P); 30 | 31 | 32 | end 33 | end 34 | 35 | function res=gfun(Xekf,t) 36 | res= 0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.4.*(t)); 37 | end 38 | 39 | function res=hfun(X,k) 40 | res=X^2/20; 41 | end -------------------------------------------------------------------------------- /2/code_0/UKF.m: -------------------------------------------------------------------------------- 1 | function Xukf=UKF(Xukf,Z,N,Q,R) 2 | 3 | %UKF滤波，UT变换 4 | L=1; %L个变量 5 | alpha=1; %0~1 6 | kalpha=0; 7 | belta=2; %建议为2 8 | ramda=3-L; 9 | for j=1:2*L+1 10 | Wm(j)=1/(2*(L+ramda)); 11 | Wc(j)=1/(2*(L+ramda)); 12 | end 13 | Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定 14 | Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定 15 | %%%%%%%%%%%%%%%% 16 | 17 | P0=eye(L);%协方差阵初始化 18 | for t=2:N 19 | xestimate=Xukf(:,t-1);%获取上一步的UKF点 20 | P=P0;%协方差阵 21 | 22 | %(1) 获得一组Sigma点集 23 | cho=(chol(P*(L+ramda)))'; 24 | for k=1:L 25 | xgamaP1(:,k)=xestimate+cho(:,k); 26 | xgamaP2(:,k)=xestimate-cho(:,k); 27 | end 28 | Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点 29 | 30 | %(2) 对Sigma点集进行一步预测 31 | Xsigmapre=gfun(Xsigma,t); 32 | 33 | %（3）计算加权均值 34 | Xpred=zeros(L,1); 35 | for k=1:2*L+1 36 | Xpred=Xpred+Wm(k)*Xsigmapre(:,k);%均值 37 | end 38 | %（4）计算加权方差 39 | Ppred=zeros(L,L); 40 | for k=1:2*L+1 41 | Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';%协方差矩阵 42 | end 43 | Ppred=Ppred+Q; 44 | 45 | %（5）根据预测值，再一次使用UT变换，得到新的sigma点集 46 | chor=(chol((L+ramda)*Ppred))'; 47 | for k=1:L 48 | XaugsigmaP1(:,k)=Xpred+chor(:,k); 49 | XaugsigmaP2(:,k)=Xpred-chor(:,k); 50 | end 51 | Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; 52 | 53 | %（6）观测预测 54 | for k=1:2*L+1 55 | Zsigmapre(1,k)=hfun(Xaugsigma(:,k),k); 56 | end 57 | 58 | %（7）计算观测预测加权均值 59 | Zpred=0; 60 | for k=1:2*L+1 61 | Zpred=Zpred+Wm(k)*Zsigmapre(:,k); 62 | end 63 | %（8）计算观测加权方差 64 | Pzz=0; 65 | for k=1:2*L+1 66 | Pzz=Pzz+Wc(k)*(Zsigmapre(:,k)-Zpred)*(Zsigmapre(:,k)-Zpred)'; 67 | end 68 | Pzz=Pzz+R; 69 | 70 | %（9）计算预测协方差 71 | Pxz=zeros(L,1); 72 | for k=1:2*L+1 73 | Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(:,k)-Zpred)'; 74 | end 75 | 76 | %（10）计算kalman增益 77 | K=Pxz*Pzz^-1; 78 | 79 | %（11）状态更新 80 | Xukf(:,t)=Xpred+K*(Z(t)-Zpred); 81 | 82 | %（12）方差更新 83 | P0=Ppred-K*Pzz*K'; 84 | end 85 | end 86 | 87 | function res=gfun(Xekf,k) 88 | res=0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.4.*(k)); 89 | end 90 | 91 | function res=hfun(X,k) 92 | res=X^2/20; 93 | end -------------------------------------------------------------------------------- /2/code_0/UKF_example.m: -------------------------------------------------------------------------------- 1 | %%% 2 | % UKF在目标跟踪中的应用 3 | %%% 4 | clear;clc; close all; 5 | N = 100; 6 | Q = 1;w=sqrt(Q)*randn(1,N); 7 | R = 1;v=sqrt(R)*randn(1,N); 8 | P =eye(1); 9 | X=zeros(1,N); 10 | s=zeros(1,N); 11 | Xukf=zeros(1,N); 12 | X(1,1)=0.1; 13 | s(1,1)=0.1; 14 | Xukf(1,1)=X(1,1); 15 | Z=zeros(1,N); 16 | Z(1)=hfun(X(1,1))+v(1); 17 | 18 | for k = 2 : N 19 | % 模拟系统 20 | s(:,k) = gfun(s(:,k-1),k-1); 21 | X(:,k) = gfun(X(:,k-1),k-1)+ w(k-1); 22 | Z(k) =hfun(X(:,k),k)+ v(k); 23 | end 24 | 25 | %UKF滤波，UT变换 26 | L=1;%4个变量 27 | alpha=1; 28 | kalpha=0; 29 | belta=2; 30 | ramda=3-L; 31 | for j=1:2*L+1 32 | Wm(j)=1/(2*(L+ramda)); 33 | Wc(j)=1/(2*(L+ramda)); 34 | end 35 | Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定 36 | Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定 37 | %%%%%%%%%%%%%%%% 38 | Xukf=zeros(L,N); 39 | Xukf(:,1)=X(:,1);%把X真值的初次数据赋给Xukf 40 | P0=eye(L);%协方差阵初始化 41 | for t=2:N 42 | xestimate=Xukf(:,t-1);%获取上一步的UKF点 43 | P=P0;%协方差阵 44 | %第一步：获得一组Sigma点集 45 | cho=(chol(P*(L+ramda)))'; 46 | for k=1:L 47 | xgamaP1(:,k)=xestimate+cho(:,k); 48 | xgamaP2(:,k)=xestimate-cho(:,k); 49 | end 50 | Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点 51 | %第二步：对Sigma点集进行一步预测 52 | Xsigmapre=gfun(Xsigma,t);%F是状态转移矩阵 53 | %第三步：利用第二步的结果计算均值和协方差 54 | Xpred=zeros(L,1); 55 | for k=1:2*L+1 56 | Xpred=Xpred+Wm(k)*Xsigmapre(:,k);%均值 57 | end 58 | Ppred=zeros(L,L); 59 | for k=1:2*L+1 60 | Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';%协方差矩阵 61 | end 62 | Ppred=Ppred+Q; 63 | %第四步：根据预测值，再一次使用UT变换，得到新的sigma点集 64 | chor=(chol((L+ramda)*Ppred))'; 65 | for k=1:L 66 | XaugsigmaP1(:,k)=Xpred+chor(:,k); 67 | XaugsigmaP2(:,k)=Xpred-chor(:,k); 68 | end 69 | Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; 70 | %第五步：观测预测 71 | for k=1:2*L+1 72 | Zsigmapre(1,k)=hfun(Xaugsigma(:,k)); 73 | end 74 | %第六步：计算观测预测均值和协方差 75 | Zpred=0; 76 | for k=1:2*L+1 77 | Zpred=Zpred+Wm(k)*Zsigmapre(1,k); 78 | end 79 | Pzz=0; 80 | for k=1:2*L+1 81 | Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)'; 82 | end 83 | Pzz=Pzz+R; 84 | 85 | Pxz=zeros(L,1); 86 | for k=1:2*L+1 87 | Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)'; 88 | end 89 | %第七步：计算kalman增益 90 | K=Pxz*(Pzz)^(-1); 91 | %第八步：状态和方差更新 92 | xestimate=Xpred+K*(Z(:,t)-Zpred); 93 | P=Ppred-K*Pzz*K'; 94 | P0=P; 95 | Xukf(:,t)=xestimate; 96 | end 97 | 98 | %误差分析 99 | for i=1:N 100 | Err_KalmanFilter(i)=RMS(X(:,i),Xukf(:,i)); 101 | end 102 | %%%%%%%%%%% 103 | %画图 104 | figure 105 | hold on ;box on 106 | plot(Z,'o'); 107 | plot(X,'-k.'); 108 | plot(Xukf,'-r+'); 109 | legend('观测轨迹','真实轨迹','UKF轨迹') 110 | figure 111 | hold on ; box on 112 | plot(Err_KalmanFilter,'-ks','MarkerFace','r') 113 | 114 | %%%%%%%%%%%%% 115 | %子函数 116 | % 计算欧氏距离子函数 117 | function dist=RMS(X1,X2) 118 | if length(X2)<=2 119 | dist=sqrt((X1(1)-X2(1))^2); 120 | else 121 | dist=sqrt((X1(1)-X2(1))^2); 122 | end 123 | end 124 | 125 | function res=gfun(Xekf,k) 126 | res=0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.5.*(k)); 127 | end 128 | 129 | function res=hfun(X,k) 130 | res=X^2/20; 131 | end 132 | %%%%%%%%%%%%% -------------------------------------------------------------------------------- /2/code_0/compare.m: -------------------------------------------------------------------------------- 1 | %EKF UKF PF 的三个算法 2 | 3 | clear; 4 | 5 | %tic; 6 | 7 | x =0.1; %初始状态 8 | 9 | x_estimate=1;%状态的估计 10 | 11 | e_x_estimate=x_estimate;%EKF的初始估计 12 | 13 | u_x_estimate=x_estimate;%UKF的初始估计 14 | 15 | p_x_estimate=x_estimate;%PF的初始估计 16 | 17 | Q =10;%input('请输入过程噪声方差 Q 的值 :'); %过程状态协方差 18 | 19 | R =1;%input('请输入测量噪声方差 R 的值 :'); %测量噪声协方差 20 | 21 | P =5;%初始估计方差 22 | 23 | e_P=P; %UKF方差 24 | 25 | u_P=P;%UKF方差 26 | 27 | pf_P=P;%PF方差 28 | 29 | tf =50; %模拟长度 30 | 31 | x_array=[x];%真实值数组 32 | 33 | e_x_estimate_array=[e_x_estimate];%EKF最优估计值数组 34 | 35 | u_x_estimate_array=[u_x_estimate];%UKF最优估计值数组 36 | 37 | p_x_estimate_array=[p_x_estimate];%PF最优估计值数组 38 | 39 | u_k=1; %微调参数 40 | 41 | u_symmetry_number=4; %对称的点的个数 42 | 43 | u_total_number=2*u_symmetry_number+1; %总的采样点的个数 44 | 45 | linear =0.5; 46 | 47 | N =500; %粒子滤波的粒子数 48 | 49 | close all; 50 | 51 | %粒子滤波初始 N 个粒子 52 | 53 | for i =1:N 54 | 55 | p_xpart(i)=p_x_estimate+sqrt(pf_P)*randn; 56 | 57 | end 58 | 59 | for k =1:tf 60 | 61 | %模拟系统 62 | 63 | x =linear *x +(25*x /(1+x^2))+8*cos(1.2*(k-1))+sqrt(Q)*randn; %状态值 64 | 65 | y =(x^2/20) +sqrt(R)*randn; %观测值 66 | 67 | %扩展卡尔曼滤波器 68 | 69 | %进行估计 第一阶段的估计 70 | 71 | e_x_estimate_1=linear *e_x_estimate+25*e_x_estimate/(1+e_x_estimate^2)+8*cos(1.2*(k-1)); 72 | 73 | e_y_estimate=(e_x_estimate_1)^2/20;%这是根据 k=1时估计值为 1得到的观测值;只是这个由 我估计得到的 第 24行的 y 也是观测值 不过是由加了噪声的真实值得到的 74 | 75 | %相关矩阵 76 | 77 | e_A=linear +25*(1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵 78 | 79 | e_H=e_x_estimate_1/10;%观测矩阵 80 | 81 | %估计的误差 82 | 83 | e_p_estimate=e_A*e_P*e_A'+Q; 84 | 85 | %扩展卡尔曼增益 86 | 87 | e_K=e_p_estimate*e_H'/(e_H*e_p_estimate*e_H'+R); 88 | 89 | %进行估计值的更新 第二阶段 90 | 91 | e_x_estimate_2=e_x_estimate_1+e_K*(y-e_y_estimate); 92 | 93 | %更新后的估计值的误差 94 | 95 | e_p_estimate_update=e_p_estimate-e_K*e_H*e_p_estimate; 96 | 97 | %进入下一次迭代的参数变化 98 | 99 | e_P=e_p_estimate_update; 100 | 101 | e_x_estimate=e_x_estimate_2; 102 | 103 | %粒子滤波器 104 | 105 | %粒子滤波器 106 | 107 | for i =1:N 108 | 109 | p_xpartminus(i)=0.5*p_xpart(i)+25*p_xpart(i)/(1+p_xpart(i)^2)+8*cos(1.2*(k-1))+ sqrt(Q)*randn; %这个式子比下面一行的效果好 110 | 111 | %xpartminus(i)=0.5*xpart(i)+25*xpart(i)/(1+xpart(i)^2)+8*cos(1.2*(k-1)); 112 | 113 | p_ypart=p_xpartminus(i)^2/20; %预测值 114 | 115 | p_vhat=y -p_ypart;%观测和预测的差 116 | 117 | p_q(i)=(1/sqrt(R)/sqrt(2*pi))*exp(-p_vhat^2/2/R); %各个粒子的权值 118 | 119 | end 120 | 121 | %平均每一个估计的可能性 122 | 123 | p_qsum=sum(p_q); 124 | 125 | for i =1:N 126 | 127 | p_q(i)=p_q(i)/p_qsum;%各个粒子进行权值归一化 128 | 129 | end 130 | 131 | %重采样 权重大的粒子多采点,权重小的粒子少采点 , 相当于每一次都进行重采样; 132 | 133 | for i =1:N 134 | 135 | p_u=rand; 136 | 137 | p_qtempsum=0; 138 | 139 | for j =1:N 140 | 141 | p_qtempsum=p_qtempsum+p_q(j); 142 | 143 | if p_qtempsum>=p_u 144 | 145 | p_xpart(i)=p_xpartminus(j);%在这里 xpart(i)实现循环赋值;终于找到了这里! ! ! break; 146 | 147 | end 148 | 149 | end 150 | 151 | end 152 | 153 | p_x_estimate=mean(p_xpart); 154 | 155 | %p_x_estimate=0; 156 | 157 | %for i =1:N 158 | 159 | %p_x_estimate=p_x_estimate+p_q(i)*p_xpart(i); 160 | 161 | %end 162 | 163 | %不敏卡尔曼滤波器 164 | 165 | %采样点的选取 存在 x(i) 166 | 167 | u_x_par=u_x_estimate; 168 | 169 | for i =2:(u_symmetry_number+1) 170 | 171 | u_x_par(i,:)=u_x_estimate+sqrt((u_symmetry_number+u_k)*u_P); 172 | 173 | end 174 | 175 | for i =(u_symmetry_number+2):u_total_number 176 | 177 | u_x_par(i,:)=u_x_estimate-sqrt((u_symmetry_number+u_k)*u_P); 178 | 179 | end 180 | 181 | %计算权值 182 | 183 | u_w_1=u_k/(u_symmetry_number+u_k); 184 | 185 | u_w_N1=1/(2*(u_symmetry_number+u_k)); 186 | 187 | %把这些粒子通过传递方程 得到下一个状态 188 | 189 | for i =1:u_total_number 190 | u_x_par(i)=0.5*u_x_par(i)+25*u_x_par(i)/(1+u_x_par(i)^2)+8*cos(1.2*(k-1)); 191 | 192 | end 193 | 194 | %传递后的均值和方差 195 | 196 | u_x_next=u_w_1*u_x_par(1); 197 | 198 | for i =2:u_total_number 199 | 200 | u_x_next=u_x_next+u_w_N1*u_x_par(i); 201 | 202 | end 203 | 204 | u_p_next=Q +u_w_1*(u_x_par(1)-u_x_next)*(u_x_par(1)-u_x_next)'; 205 | 206 | for i =2:u_total_number 207 | 208 | u_p_next=u_p_next+u_w_N1*(u_x_par(i)-u_x_next)*(u_x_par(i)-u_x_next)'; 209 | 210 | end 211 | 212 | %%对传递后的均值和方差进行采样 产生粒子 存在 y(i) 213 | 214 | %u_y_2obser(1)=u_x_next; 215 | 216 | %for i =2:(u_symmetry_number+1) 217 | 218 | %u_y_2obser(i,:)=u_x_next+sqrt((u_symmetry_number+k)*u_p_next); %end 219 | 220 | %for i =(u_symmetry_number+2) :u_total_number 221 | 222 | %u_y_2obser(i,:)=u_x_next-sqrt((u_symmetry_number+u_k)*u_p_next); %end 223 | 224 | %另外存在 y_2obser(i)中; 225 | 226 | for i =1:u_total_number 227 | 228 | u_y_2obser(i,:)=u_x_par(i); 229 | 230 | end 231 | 232 | %通过观测方程 得到一系列的粒子 233 | 234 | for i =1:u_total_number 235 | 236 | u_y_2obser(i)=u_y_2obser(i)^2/20; 237 | 238 | end 239 | 240 | %通过观测方程后的均值 y_obse 241 | 242 | u_y_obse=u_w_1*u_y_2obser(1); 243 | 244 | for i =2:u_total_number 245 | 246 | u_y_obse=u_y_obse+u_w_N1*u_y_2obser(i); 247 | 248 | end 249 | 250 | %Pzz测量方差矩阵 251 | 252 | u_pzz=R +u_w_1*(u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)'; 253 | 254 | for i =2:u_total_number 255 | 256 | u_pzz=u_pzz+u_w_N1*(u_y_2obser(i)-u_y_obse)*(u_y_2obser(i)-u_y_obse)'; 257 | 258 | end 259 | 260 | %Pxz状态向量与测量值的协方差矩阵 261 | 262 | u_pxz=u_w_1*(u_x_par(1)-u_x_next)*(u_y_2obser(1)-u_y_obse)'; 263 | 264 | for i =2:u_total_number 265 | 266 | u_pxz=u_pxz+u_w_N1*(u_x_par(i)-u_x_next)*(u_y_2obser(i)-u_y_obse)'; 267 | 268 | end 269 | 270 | %卡尔曼增益 271 | 272 | u_K=u_pxz/u_pzz; 273 | 274 | %估计量的更新 275 | 276 | u_x_next_optimal=u_x_next+u_K*(y-u_y_obse);%第一步的估计值 +修正值; u_x_estimate=u_x_next_optimal; 277 | 278 | %方差的更新 279 | 280 | u_p_next_update=u_p_next-u_K*u_pzz*u_K'; 281 | 282 | u_P=u_p_next_update; 283 | 284 | %进行画图程序 285 | 286 | x_array=[x_array,x]; 287 | 288 | e_x_estimate_array=[e_x_estimate_array,e_x_estimate]; 289 | 290 | p_x_estimate_array=[p_x_estimate_array,p_x_estimate]; 291 | 292 | u_x_estimate_array=[u_x_estimate_array,u_x_estimate]; 293 | 294 | e_error(k,:)=abs(x_array(k)-e_x_estimate_array(k)); 295 | 296 | p_error(k,:)=abs(x_array(k)-p_x_estimate_array(k)); 297 | 298 | u_error(k,:)=abs(x_array(k)-u_x_estimate_array(k)); 299 | 300 | end 301 | 302 | t =0:tf; 303 | 304 | figure; 305 | 306 | plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g--',t,u_x_estimate_array,'b:'); 307 | 308 | set(gca,'FontSize',10); 309 | 310 | set(gcf,'color','White'); 311 | 312 | xlabel('时间步长 ');%lable --->label我的神 313 | 314 | ylabel('状态 '); 315 | 316 | legend('真实值 ','EKF 估计值 ','PF 估计值 ','UKF 估计值 '); 317 | 318 | figure; 319 | 320 | plot(t,x_array,'k.',t,p_x_estimate_array,'g--',t, p_x_estimate_array-1.96*sqrt(P),'r:',t, p_x_estimate_array+1.96*sqrt(P),'r:'); 321 | 322 | set(gca,'FontSize',10); 323 | 324 | set(gcf,'color','White'); 325 | 326 | xlabel('时间步长 ');%lable --->label我的神 327 | 328 | ylabel('状态 '); 329 | 330 | legend('真实值 ','PF 估计值 ', '95%置信区间 '); 331 | 332 | %rootmean square 平均值的平方根 333 | 334 | e_xrms=sqrt((norm(x_array-e_x_estimate_array)^2)/tf); 335 | 336 | disp(['EKF估计误差均方值 =',num2str(e_xrms)]); 337 | 338 | p_xrms=sqrt((norm(x_array-p_x_estimate_array)^2)/tf); 339 | 340 | disp(['PF估计误差均方值 =',num2str(p_xrms)]); 341 | 342 | u_xrms=sqrt((norm(x_array-u_x_estimate_array)^2)/tf); 343 | 344 | disp(['UKF估计误差均方值 =',num2str(u_xrms)]); 345 | 346 | %plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:'); 347 | 348 | %legend('EKF估计值误差 ','PF 估计值误差 ','UKF 估计值误差 '); 349 | 350 | t =1:tf; 351 | 352 | figure; 353 | 354 | plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:'); 355 | 356 | set(gca,'FontSize',10); 357 | 358 | set(gcf,'color','White'); 359 | 360 | xlabel('时间步长 ');%lable --->label我的神 361 | 362 | ylabel('状态 '); 363 | 364 | legend('EKF估计值误差 ','PF 估计值误差 ','UKF 估计值误差 '); 365 | 366 | %toc; -------------------------------------------------------------------------------- /2/code_0/main.m: -------------------------------------------------------------------------------- 1 | 2 | %% 匀速直线运动 3 | clc;clear; 4 | close all; 5 | T=1;%雷达扫描周期 6 | N=25/T;%总的采样次数 7 | X=zeros(2,N);%目标真实位置、速度 8 | X(:,1)=[0,2];%目标初始位置、速度 9 | S(:,1)=[0,2]; 10 | Z=zeros(1,N);%传感器对位置的观测 11 | Z(:,1)=X(1,1);%观测初始化 12 | delta_w=1e-1; %如果增大这个参数，目标真实轨迹就是曲线了 13 | 14 | Q=delta_w*diag([2,2]);%过程噪声方差 15 | R=eye(1)*10;%观测噪声均值 16 | 17 | phi=[1,T;0,1];%状态转移矩阵 18 | Tau=[T^2/2 0;0,T]; 19 | u=[1;1]; 20 | W=[1;0]; 21 | H=[1,0];%观测矩阵 22 | Xn=zeros(2,0); 23 | Xn_fix=zeros(2,0); 24 | 25 | for i=2:N 26 | S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹 27 | X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹 28 | Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测 29 | end 30 | 31 | % Kalman 滤波 32 | Xkf=zeros(2,N); 33 | Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 34 | Xkf_gainfix=Xkf; 35 | P0=100e-2*eye(2);%协方差阵初始化 36 | %% 基本离散kalman滤波 37 | tic; 38 | Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N); 39 | toc; 40 | 41 | %% 固定增益的kalman滤波 42 | tic; 43 | Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N); 44 | toc; 45 | 46 | %误差分析 47 | for i=1:N 48 | 49 | Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 50 | Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差 51 | Err_fixKalmanFilter(i)=RMS(X(:,i),Xkf_gainfix(:,i));%滤波后的误差 52 | end 53 | mean_Observation=mean(Err_Observation); 54 | mean_KalmanFilter=mean(Err_KalmanFilter); 55 | mean_fixKalmanFilter=mean(Err_fixKalmanFilter); 56 | figure 57 | hold on;box on; 58 | t=(0:1:N-1); 59 | plot(t,S(1,:),'g','LineWidth',1);%理论轨迹 60 | plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 61 | plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 62 | plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 63 | plot(t,Xkf_gainfix(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹 64 | % M=M'; 65 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 66 | legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','固定增益滤波后轨迹'); 67 | xlabel('横坐标 T/s'); 68 | ylabel('纵坐标 X/m'); 69 | 70 | figure 71 | hold on;box on; 72 | plot(t,Err_Observation,'-'); 73 | plot(t,Err_KalmanFilter,'--'); 74 | plot(t,Err_fixKalmanFilter,'-.'); 75 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 76 | legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),sprintf('固定增益滤波后误差%.03f',mean_fixKalmanFilter)); 77 | xlabel('观测时间/s'); 78 | ylabel('误差值'); 79 | 80 | 81 | % 计算欧氏距离子函数 82 | function dist=RMS(X1,X2) 83 | if length(X2)<=2 84 | dist=sqrt((X1(1)-X2(1))^2); 85 | else 86 | dist=sqrt((X1(1)-X2(1))^2); 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /2/code_0/main_UKF.m: -------------------------------------------------------------------------------- 1 | %%% 2 | clear;clc; close all; 3 | N = 50; 4 | Q = 1;w=sqrt(Q)*randn(1,N); 5 | R = 1;v=sqrt(R)*randn(1,N); 6 | P =eye(1); 7 | X=zeros(1,N); 8 | s=zeros(1,N); 9 | Xukf=zeros(1,N); 10 | X(1,1)=0.1; 11 | s(1,1)=0.1; 12 | Xukf(1,1)=X(1,1); 13 | Z=zeros(1,N); 14 | Z(1)=hfun(X(1,1))+v(1); 15 | 16 | for k = 2 : N 17 | % 模拟系统 18 | s(:,k) = gfun(s(:,k-1),k-1); 19 | X(:,k) = gfun(X(:,k-1),k-1)+ w(k-1); 20 | Z(k) =hfun(X(:,k),k)+ v(k); 21 | end 22 | 23 | %UKF滤波，UT变换 24 | L=1; %L个变量 25 | alpha=1; %0~1 26 | kalpha=0; 27 | belta=2; %建议为2 28 | ramda=3-L; 29 | for j=1:2*L+1 30 | Wm(j)=1/(2*(L+ramda)); 31 | Wc(j)=1/(2*(L+ramda)); 32 | end 33 | Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定 34 | Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定 35 | %%%%%%%%%%%%%%%% 36 | Xukf = zeros(L,N); 37 | Xukf(:,1) = X(:, 1); 38 | P0=eye(L);%协方差阵初始化 39 | for t=2:N 40 | xestimate=Xukf(:,t-1);%获取上一步的UKF点 41 | P=P0;%协方差阵 42 | 43 | %(1) 获得一组Sigma点集 44 | cho=(chol(P*(L+ramda)))'; 45 | for k=1:L 46 | xgamaP1(:,k)=xestimate+cho(:,k); 47 | xgamaP2(:,k)=xestimate-cho(:,k); 48 | end 49 | Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点 50 | 51 | %(2) 对Sigma点集进行一步预测 52 | Xsigmapre=gfun(Xsigma,t); 53 | 54 | %（3）计算加权均值 55 | Xpred=zeros(L,1); 56 | for k=1:2*L+1 57 | Xpred=Xpred+Wm(k)*Xsigmapre(:,k);%均值 58 | end 59 | %（4）计算加权方差 60 | Ppred=zeros(L,L); 61 | for k=1:2*L+1 62 | Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';%协方差矩阵 63 | end 64 | Ppred=Ppred+Q; 65 | 66 | %（5）根据预测值，再一次使用UT变换，得到新的sigma点集 67 | chor=(chol((L+ramda)*Ppred))'; 68 | for k=1:L 69 | XaugsigmaP1(:,k)=Xpred+chor(:,k); 70 | XaugsigmaP2(:,k)=Xpred-chor(:,k); 71 | end 72 | Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; 73 | 74 | %（6）观测预测 75 | for k=1:2*L+1 76 | Zsigmapre(1,k)=hfun(Xaugsigma(:,k),k); 77 | end 78 | 79 | %（7）计算观测预测加权均值 80 | Zpred=0; 81 | for k=1:2*L+1 82 | Zpred=Zpred+Wm(k)*Zsigmapre(:,k); 83 | end 84 | %（8）计算观测加权方差 85 | Pzz=0; 86 | for k=1:2*L+1 87 | Pzz=Pzz+Wc(k)*(Zsigmapre(:,k)-Zpred)*(Zsigmapre(:,k)-Zpred)'; 88 | end 89 | Pzz=Pzz+R; 90 | 91 | %（9）计算预测协方差 92 | Pxz=zeros(L,1); 93 | for k=1:2*L+1 94 | Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(:,k)-Zpred)'; 95 | end 96 | 97 | %（10）计算kalman增益 98 | K=Pxz*Pzz^-1; 99 | 100 | %（11）状态更新 101 | Xukf(:,t)=Xpred+K*(Z(t)-Zpred); 102 | 103 | %（12）方差更新 104 | P0=Ppred-K*Pzz*K'; 105 | end 106 | 107 | 108 | for i=1:N 109 | Err_Obs(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 110 | Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差 111 | end 112 | mean_Obs=mean(Err_Obs); 113 | mean_UKF=mean(Err_UKF); 114 | 115 | %%%%%%%%%%% 116 | %画图 117 | figure 118 | hold on ;box on 119 | t=(0:1:N-1); 120 | plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 121 | % plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 122 | plot(t,Xukf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 123 | % plot(X(1,:),X(3,:),'-k.'); 124 | % plot(Xukf(1,:),Xukf(3,:),'-r+'); 125 | legend('真实轨迹','UKF轨迹'); 126 | xlabel('横坐标 T/s'); 127 | ylabel('纵坐标 X/m'); 128 | 129 | figure 130 | hold on;box on; 131 | plot(t,Err_Obs,'-'); 132 | plot(t,Err_UKF,'--'); 133 | % plot(t,Err_EKF1,'-.'); 134 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 135 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF)); 136 | xlabel('观测时间/s'); 137 | ylabel('误差值'); 138 | 139 | %%%%%%%%%%%%% 140 | %子函数 141 | % 计算欧氏距离子函数 142 | function dist=RMS(X1,X2) 143 | if length(X2)<=2 144 | dist=sqrt((X1(1)-X2(1))^2); 145 | else 146 | dist=sqrt((X1(1)-X2(1))^2); 147 | end 148 | end 149 | 150 | function res=gfun(Xekf,k) 151 | res=0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.3.*(k)); 152 | end 153 | 154 | function res=hfun(X,k) 155 | res=X^2/20; 156 | end 157 | %%%%%%%%%%%%% -------------------------------------------------------------------------------- /2/code_0/main_UKF_PF.m: -------------------------------------------------------------------------------- 1 | clear;clc; close all; 2 | %% initialize the variables 3 | N = 100; % 共进行75次 4 | N=N-1; 5 | M = 100; % 粒子数，越大效果越好，计算量也越大 6 | x = 0.1; % initial actual state 7 | Q = 1;w=sqrt(Q)*randn(1,N); 8 | R = 1;v=sqrt(R)*randn(1,N); 9 | 10 | 11 | X=zeros(1,N); 12 | s=zeros(1,N); 13 | Xpf=zeros(1,N); 14 | Xukf=zeros(1,N); 15 | X(1,1)=0.1; 16 | s(1,1)=0.1; 17 | Xpf(1,1)=X(1,1); 18 | Xukf(1,1)=X(1,1); 19 | Z=zeros(1,N); 20 | Z(1)=X(1,1)^2/20+v(1); 21 | 22 | for k = 2 : N+1 23 | % 模拟系统 24 | s(:,k) = gfun(s(:,k-1),k-1); 25 | X(:,k) = gfun(X(:,k-1),k-1)+ sqrt(Q)*randn; 26 | Z(:,k) =hfun(X(:,k),k)+ sqrt(R)*randn; 27 | end 28 | 29 | %粒子滤波pf 30 | tic; 31 | Xpf=PF(Xpf,Z,Q,R,M,N); 32 | toc; 33 | tic; 34 | Xukf=UKF(Xukf,Z,N+1,Q,R); 35 | toc; 36 | 37 | for i=1:N+1 38 | Err_Obs(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 39 | Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差 40 | Err_PF(i)=RMS(X(:,i),Xpf(:,i));%滤波后的误差 41 | end 42 | mean_Obs=mean(Err_Obs); 43 | mean_UKF=mean(Err_UKF); 44 | mean_PF=mean(Err_PF); 45 | 46 | %%%%%%%%%%% 47 | %画图 48 | figure 49 | hold on ;box on 50 | t=(0:1:N); 51 | plot(t,X(1,:),'-r','LineWidth',1);%实际轨迹 52 | plot(t,Xukf(1,:),'-.k','LineWidth',1);%卡尔曼滤波轨迹 53 | plot(t,Xpf(1,:),'-.ob','LineWidth',1);%观测轨迹 54 | % plot(X(1,:),X(3,:),'-k.'); 55 | legend('真实轨迹','UKF轨迹','PF轨迹'); 56 | xlabel('横坐标 T/s'); 57 | ylabel('纵坐标 X/m'); 58 | 59 | figure 60 | hold on;box on; 61 | plot(t,Err_Obs,'-'); 62 | plot(t,Err_UKF,'--'); 63 | plot(t,Err_PF,'-.'); 64 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 65 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF),sprintf('PF滤波后误差%.03f',mean_PF)); 66 | xlabel('观测时间/s'); 67 | ylabel('误差值'); 68 | 69 | %%%%%%%%%%%%% 70 | %子函数 71 | % 计算欧氏距离子函数 72 | function dist=RMS(X1,X2) 73 | if length(X2)<=2 74 | dist=sqrt((X1(1)-X2(1))^2); 75 | else 76 | dist=sqrt((X1(1)-X2(1))^2); 77 | end 78 | end 79 | function res=gfun(Xekf,t) 80 | res= 0.5*Xekf + 25*Xekf/(1 + Xekf^2) + 8*cos(0.4*(t)); 81 | end 82 | 83 | function res=hfun(X,k) 84 | res=X^2/20; 85 | end 86 | 87 | -------------------------------------------------------------------------------- /2/code_1/center_federated_filter.m: -------------------------------------------------------------------------------- 1 | %% 2 | clc 3 | clear all; 4 | close all; 5 | %1.建一个匀速圆周运动的模型 6 | 7 | r = 500; 8 | x = -r:0.01:r; 9 | y = sqrt(r.^2 - x.^2); %运动的轨迹 10 | 11 | % plot(x,[y;-y],'LineWidth',2); 12 | % grid on;axis equal;title('质点轨迹','FontSize',16) 13 | % xlabel('x/m','FontSize',16) 14 | % ylabel('y/m','FontSize',16) 15 | % xlim([-2000 2000]) 16 | % ylim([-2000 2000]) 17 | % text(750,750,'o','color','g') 18 | % text(750,750,'雷达1','FontSize',10) 19 | % text(-750,750,'o','color','g') 20 | % text(-750,750,'雷达2','FontSize',10) 21 | % text(0,-1000,'o','color','g') 22 | % text(0,-1000,'雷达3','FontSize',10) 23 | 24 | %% 25 | %目标的移动 26 | Observation_time = 0.5; 27 | 28 | v = 10; 29 | w = v/r;%角速度 30 | t = 0:Observation_time:1.5*pi/w;%刚好转一圈 31 | Q = 2;%过程噪声 32 | x_w=sqrt(Q)*randn(1,size(t,2)); 33 | y_w=sqrt(Q)*randn(1,size(t,2)); 34 | 35 | x_traj = r * sin(w*t);%实现刚好走完一圈 36 | y_traj = r * cos(w*t); 37 | s_traj = [x_traj;y_traj]; 38 | 39 | x_ture_traj = r * sin(w*t)+x_w;%实现刚好走完一圈 40 | y_ture_traj = r * cos(w*t)+y_w; 41 | ture_traj = [x_ture_traj;y_ture_traj]; 42 | 43 | 44 | %雷达的位置 45 | radar_1 = [750 750]; 46 | radar_2 = [-750 750]; 47 | radar_3 = [0 -1000]; 48 | radars=[radar_1;radar_2;radar_3]; 49 | radar1_noise = 3; 50 | radar2_noise = 1; 51 | radar3_noise = 4; 52 | radars_noise=diag([3,1,4]); 53 | 54 | %真实的测量信号 55 | radar1_ture_measuremnet = sqrt((radar_1(1)-x_ture_traj).^2 + (radar_1(2)-y_ture_traj).^2 ); 56 | radar2_ture_measuremnet = sqrt((radar_2(1)-x_ture_traj).^2 + (radar_2(2)-y_ture_traj).^2 ); 57 | radar3_ture_measuremnet = sqrt((radar_3(1)-x_ture_traj).^2 + (radar_3(2)-y_ture_traj).^2 ); 58 | 59 | %加噪声的测量 60 | radar1_noise_measuremnet = radar1_ture_measuremnet + radar1_noise * randn(1,size(radar1_ture_measuremnet,2)); 61 | radar2_noise_measuremnet = radar2_ture_measuremnet + radar2_noise * randn(1,size(radar2_ture_measuremnet,2)); 62 | radar3_noise_measuremnet = radar3_ture_measuremnet + radar3_noise * randn(1,size(radar3_ture_measuremnet,2)); 63 | radars_noise_measuremnet=[radar1_noise_measuremnet;radar2_noise_measuremnet;radar3_noise_measuremnet]; 64 | 65 | 66 | %% 67 | %FR结构的联邦滤波器(融合重置结构) 68 | %联邦滤波器 三个子滤波器 一个主滤波器 69 | beta_M = 0; 70 | N = 3; 71 | beta_i = 1/N;%每个传感器分配的权值是一样的 72 | 73 | Q_process_noise_system = diag([2 2]);%系统的过程噪声，题目的要求 74 | QQ=blkdiag(Q_process_noise_system,Q_process_noise_system,Q_process_noise_system); 75 | P_cov_system = diag([1000 1000 ]);%初始的系统协方差,随便给定的 76 | X_init_state = [0;500];%初始目标的位置 77 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%1.信息分配与重置%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 78 | 79 | Q_process_noise_radar = 1/(beta_i) * Q_process_noise_system; 80 | 81 | system_init_pos = X_init_state; 82 | object1_init_pos = X_init_state; 83 | object2_init_pos = X_init_state; 84 | object3_init_pos = X_init_state; 85 | objects_init_pos=[object1_init_pos;object2_init_pos;object3_init_pos]; 86 | 87 | object1_correct_pos = X_init_state; 88 | object2_correct_pos = X_init_state; 89 | object3_correct_pos = X_init_state; 90 | fusion_pos = X_init_state; 91 | central_fusion_pos = X_init_state; 92 | 93 | object1_init_cov = 1/(beta_i) * P_cov_system; 94 | object2_init_cov = 1/(beta_i) * P_cov_system; 95 | object3_init_cov = 1/(beta_i) * P_cov_system; 96 | objects_init_cov = blkdiag(object1_init_cov,object2_init_cov,object3_init_cov); 97 | 98 | %集中式卡尔曼 99 | tic; 100 | for i = 1:size(radar1_ture_measuremnet,2)-1 %第一个观测值扔掉 101 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%2.传感器时间量测更新%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 | 103 | %由于是匀速圆周运动，所以可以用旋转矩阵来做预测 R = [cos(theta) -sin(theta) ; sin(theta) cos(theta)] 104 | %由于旋转矩阵本身的方向是逆时针方向的，而本题物体的运动是瞬时间的，所以需要用到旋转矩阵的逆 = 旋转矩阵的转置 105 | theta = w * Observation_time * 1; 106 | R = [cos(theta) -sin(theta); 107 | sin(theta) cos(theta)]'; %对于每个传感器都是一样的 108 | %预测 109 | F = R; 110 | FF=blkdiag(R,R,R); 111 | objects_estimate_pos(:,i+1) = FF * objects_init_pos;%集中滤波器 112 | P_objects_estimate = FF * objects_init_cov * FF' + QQ;%集中滤波器 113 | 114 | %更新 115 | %因为观测并不是线性的，所以这里需要用到拓展卡尔曼滤波，对观测方程进行求导 116 | %观测矩阵也是不断变化的，因为是在均值处进行展开 117 | dec1 = sqrt((radar_1(1)-objects_estimate_pos(1,i+1)).^2 + (radar_1(2)-objects_estimate_pos(2,i+1)).^2); 118 | dec2 = sqrt((radar_2(1)-objects_estimate_pos(3,i+1)).^2 + (radar_2(2)-objects_estimate_pos(4,i+1)).^2); 119 | dec3 = sqrt((radar_3(1)-objects_estimate_pos(5,i+1)).^2 + (radar_3(2)-objects_estimate_pos(6,i+1)).^2); 120 | decs=[dec1;dec2;dec3]; 121 | % decs= 122 | H1 =[ -(radar_1(1)-objects_estimate_pos(1,i+1))/dec1 -(radar_1(2)-objects_estimate_pos(2,i+1))/dec1]; 123 | H2 =[ -(radar_2(1)-objects_estimate_pos(3,i+1))/dec2 -(radar_2(2)-objects_estimate_pos(4,i+1))/dec2]; 124 | H3 =[ -(radar_3(1)-objects_estimate_pos(5,i+1))/dec3 -(radar_3(2)-objects_estimate_pos(6,i+1))/dec3]; 125 | HH=blkdiag(H1,H2,H3); 126 | KK = P_objects_estimate * HH' * (HH * P_objects_estimate * HH' + radars_noise)^-1 ; 127 | objects_correct_pos(:,i+1) = objects_estimate_pos(:,i+1) + KK * ( radars_noise_measuremnet(:,i+1) - decs);%集中滤波器 128 | objects_init_cov = (eye(6) - KK * HH)*P_objects_estimate;%集中滤波器 129 | central_fusion_pos(:,i+1)=[mean(objects_correct_pos([1 3 5],i+1));mean(objects_correct_pos([2 4 6],i+1))]; 130 | objects_init_pos = objects_correct_pos(:,i+1); 131 | end 132 | toc; 133 | tic; 134 | for i = 1:size(radar1_ture_measuremnet,2)-1 %第一个观测值扔掉 135 | 136 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%2.传感器时间量测更新%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 137 | 138 | %由于是匀速圆周运动，所以可以用旋转矩阵来做预测 R = [cos(theta) -sin(theta) ; sin(theta) cos(theta)] 139 | %由于旋转矩阵本身的方向是逆时针方向的，而本题物体的运动是瞬时间的，所以需要用到旋转矩阵的逆 = 旋转矩阵的转置 140 | theta = w * Observation_time * 1; 141 | R = [cos(theta) -sin(theta); 142 | sin(theta) cos(theta)]'; %对于每个传感器都是一样的 143 | %% 144 | %传感器1 145 | %嵌套 146 | %radar1的卡尔曼子滤波器 147 | %预测 148 | F = R; 149 | % FF=blkdiag(R,R,R); 150 | object1_estimate_pos(:,i+1) = F * object1_init_pos; 151 | % objects_estimate_pos(:,i+1) = FF * objects_init_pos;%集中滤波器 152 | P_object1_estimate = F * object1_init_cov * F' + Q_process_noise_radar; 153 | % P_objects_estimate = FF * objects_init_cov * FF' + QQ;%集中滤波器 154 | 155 | %更新 156 | %因为观测并不是线性的，所以这里需要用到拓展卡尔曼滤波，对观测方程进行求导 157 | %观测矩阵也是不断变化的，因为是在均值处进行展开 158 | dec = sqrt((radar_1(1)-object1_estimate_pos(1,i+1)).^2 + (radar_1(2)-object1_estimate_pos(2,i+1)).^2); 159 | % dec1 = sqrt((radar_1(1)-objects_estimate_pos(1,i+1)).^2 + (radar_1(2)-objects_estimate_pos(2,i+1)).^2); 160 | % dec2 = sqrt((radar_2(1)-objects_estimate_pos(3,i+1)).^2 + (radar_2(2)-objects_estimate_pos(4,i+1)).^2); 161 | % dec3 = sqrt((radar_3(1)-objects_estimate_pos(5,i+1)).^2 + (radar_3(2)-objects_estimate_pos(6,i+1)).^2); 162 | % decs=[dec1;dec2;dec3]; 163 | % decs= 164 | H = [ -(radar_1(1)-object1_estimate_pos(1,i+1))/dec -(radar_1(2)-object1_estimate_pos(2,i+1))/dec]; 165 | % H1 =[ -(radar_1(1)-objects_estimate_pos(1,i+1))/dec1 -(radar_1(2)-objects_estimate_pos(2,i+1))/dec1]; 166 | % H2 =[ -(radar_2(1)-objects_estimate_pos(3,i+1))/dec2 -(radar_2(2)-objects_estimate_pos(4,i+1))/dec2]; 167 | % H3 =[ -(radar_3(1)-objects_estimate_pos(5,i+1))/dec3 -(radar_3(2)-objects_estimate_pos(6,i+1))/dec3]; 168 | % HH=blkdiag(H1,H2,H3); 169 | K = P_object1_estimate * H' * (H * P_object1_estimate * H' + radar1_noise)^-1 ; 170 | % KK = P_objects_estimate * HH' * inv(HH * P_objects_estimate * HH' + radars_noise) ; 171 | object1_correct_pos(:,i+1) = object1_estimate_pos(:,i+1) + K * ( radar1_noise_measuremnet(i+1) - sqrt((radar_1(1)-object1_estimate_pos(1,i+1)).^2 + (radar_1(2)-object1_estimate_pos(2,i+1)).^2)); 172 | % objects_correct_pos(:,i+1) = objects_estimate_pos(:,i+1) + KK * ( radars_noise_measuremnet(:,i+1) - decs);%集中滤波器 173 | inform_object1_correct = inv((eye(2) - K * H)*P_object1_estimate); 174 | % objects_init_cov = (eye(6) - KK * HH)*P_objects_estimate;%集中滤波器 175 | 176 | %radar2的卡尔曼子滤波器 177 | 178 | object2_estimate_pos(:,i+1) = F * object2_init_pos; 179 | P_object2_estimate = F * object2_init_cov * F' + Q_process_noise_radar; 180 | 181 | %更新 182 | 183 | dec = sqrt((radar_2(1)-object2_estimate_pos(1,i+1)).^2 + (radar_2(2)-object2_estimate_pos(2,i+1)).^2); 184 | H = [ -(radar_2(1)-object2_estimate_pos(1,i+1))/dec -(radar_2(2)-object2_estimate_pos(2,i+1))/dec]; 185 | 186 | 187 | K = P_object2_estimate * H' * (H * P_object2_estimate * H' + radar2_noise)^-1 ; 188 | object2_correct_pos(:,i+1) = object2_estimate_pos(:,i+1) + K * ( radar2_noise_measuremnet(i+1) - sqrt((radar_2(1)-object2_estimate_pos(1,i+1)).^2 + (radar_2(2)-object2_estimate_pos(2,i+1)).^2)); 189 | inform_object2_correct = inv((eye(2) - K * H)*P_object2_estimate); 190 | 191 | %% 192 | %radar3的卡尔曼子滤波器 193 | %object3_init_pos 194 | object3_estimate_pos(:,i+1) = F * object3_init_pos; 195 | P_object3_estimate = F * object3_init_cov * F' + Q_process_noise_radar; 196 | 197 | %更新 198 | 199 | dec = sqrt((radar_3(1)-object3_estimate_pos(1,i+1)).^2 + (radar_3(2)-object3_estimate_pos(2,i+1)).^2); 200 | H = [ -(radar_3(1)-object3_estimate_pos(1,i+1))/dec -(radar_3(2)-object3_estimate_pos(2,i+1))/dec]; 201 | 202 | 203 | 204 | K = P_object3_estimate * H' * (H * P_object3_estimate * H' + radar3_noise)^-1 ; 205 | object3_correct_pos(:,i+1) = object3_estimate_pos(:,i+1) + K * ( radar3_noise_measuremnet(i+1) - sqrt( (radar_3(1)-object3_estimate_pos(1,i+1)).^2 + (radar_3(2)-object3_estimate_pos(2,i+1)).^2) ); 206 | inform_object3_correct = inv((eye(2) - K * H)*P_object3_estimate); 207 | 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%3.主滤波器时间更新%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | 210 | %主滤波器 有3种模式，NR,ZR,FS 211 | 212 | system_estimate_pos = F * system_init_pos; 213 | system_estimate_cov = F * P_cov_system * F'; 214 | 215 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%4.最后的融合处理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 216 | fusion_cov = inv( inform_object1_correct + inform_object2_correct + inform_object3_correct + inv(system_estimate_cov)); 217 | fusion_pos(:,i+1) = fusion_cov* (inform_object1_correct *object1_correct_pos(:,i+1) + inform_object2_correct * object2_correct_pos(:,i+1) + inform_object3_correct *object3_correct_pos(:,i+1) + inv(system_estimate_cov) * system_estimate_pos ); 218 | 219 | 220 | object1_init_pos = fusion_pos(:,i+1); 221 | object1_init_cov = 1/(beta_i) * fusion_cov; 222 | object2_init_pos = fusion_pos(:,i+1); 223 | object2_init_cov = 1/(beta_i) * fusion_cov; 224 | object3_init_pos = fusion_pos(:,i+1); 225 | object3_init_cov = 1/(beta_i) * fusion_cov; 226 | 227 | system_init_pos = fusion_pos(:,i+1); 228 | P_cov_system = fusion_cov ; 229 | 230 | end 231 | toc; 232 | 233 | % plot(x,[y;-y],'LineWidth',2); 234 | grid on;axis equal;title('质点轨迹','FontSize',16) 235 | xlabel('x/m','FontSize',16) 236 | ylabel('y/m','FontSize',16) 237 | xlim([-1500 1500]) 238 | ylim([-1500 1500]) 239 | text(750,750,'o','color','g') 240 | text(750,750,'传感器1','FontSize',10) 241 | text(-750,750,'o','color','g') 242 | text(-750,750,'传感器2','FontSize',10) 243 | text(0,-1000,'o','color','g') 244 | text(0,-1000,'传感器3','FontSize',10) 245 | hold on 246 | plot(fusion_pos(1,:),fusion_pos(2,:)); 247 | plot(central_fusion_pos(1,:),central_fusion_pos(2,:)); 248 | legend("联邦卡尔曼滤波","集中式滤波"); 249 | % comet(fusion_pos(1,:),fusion_pos(2,:)) 250 | % comet(central_fusion_pos(1,:),central_fusion_pos(2,:)) 251 | 252 | for i=1:size(radar1_ture_measuremnet,2)-1 253 | % Err_S(i)=Dist(ture_traj(:,i),s_traj(:,i));%滤波前的误差 254 | Err_FF(i)=Dist(fusion_pos(:,i),ture_traj(:,i));%滤波前的误差 255 | Err_CFF(i)=Dist(central_fusion_pos(:,i),ture_traj(:,i));%滤波前的误差 256 | % Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差 257 | end 258 | % mean_S=mean(Err_S); 259 | mean_FF=mean(Err_FF); 260 | mean_CFF=mean(Err_CFF); 261 | % mean_UKF=mean(Err_UKF); 262 | % mean_PF=mean(Err_PF); 263 | t=1:size(radar1_ture_measuremnet,2)-1; 264 | figure 265 | hold on;box on; 266 | % plot(t,Err_Obs,'-'); 267 | % plot(t,Err_UKF,'--'); 268 | % plot(t,Err_S,'-.'); 269 | plot(t,Err_FF,'-'); 270 | plot(t,Err_CFF,'-.'); 271 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 272 | legend(sprintf('联邦滤波后误差%.03f',mean_FF),sprintf('集中式滤波后误差%.03f',mean_CFF)); 273 | xlabel('观测时间/s'); 274 | ylabel('误差值'); 275 | 276 | function d=Dist(X1,X2) 277 | if length(X2)<=2 278 | d=sqrt((X1(1,1)-X2(1,1))^2+(X1(2,1)-X2(2,1))^2); 279 | else 280 | % d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2); 281 | end 282 | end -------------------------------------------------------------------------------- /2/code_1/federated_filter.m: -------------------------------------------------------------------------------- 1 | %% 2 | clc 3 | clear all; 4 | close all; 5 | %1.建一个匀速圆周运动的模型 6 | 7 | r = 500; 8 | x = -r:0.01:r; 9 | y = sqrt(r.^2 - x.^2); %运动的轨迹 10 | 11 | plot(x,[y;-y],'LineWidth',2); 12 | grid on;axis equal;title('质点轨迹','FontSize',16) 13 | xlabel('x/m','FontSize',16) 14 | ylabel('y/m','FontSize',16) 15 | xlim([-2000 2000]) 16 | ylim([-2000 2000]) 17 | text(750,750,'o','color','g') 18 | text(750,750,'雷达1','FontSize',10) 19 | text(-750,750,'o','color','g') 20 | text(-750,750,'雷达2','FontSize',10) 21 | text(0,-1000,'o','color','g') 22 | text(0,-1000,'雷达3','FontSize',10) 23 | 24 | %% 25 | %目标的移动 26 | Observation_time = 0.5; 27 | 28 | v = 10; 29 | w = v/r;%角速度 30 | t = 0:Observation_time:2*pi/w;%刚好转一圈 31 | Q = 2;%过程噪声 32 | x_w=sqrt(Q)*randn(1,size(t,2)); 33 | y_w=sqrt(Q)*randn(1,size(t,2)); 34 | 35 | x_traj = r * sin(w*t);%实现刚好走完一圈 36 | y_traj = r * cos(w*t); 37 | s_traj = [x_traj;y_traj]; 38 | 39 | x_ture_traj = r * sin(w*t)+x_w;%实现刚好走完一圈 40 | y_ture_traj = r * cos(w*t)+y_w; 41 | ture_traj = [x_ture_traj;y_ture_traj]; 42 | 43 | 44 | %雷达的位置 45 | radar_1 = [750 750]; 46 | radar_2 = [-750 750]; 47 | radar_3 = [0 -1000]; 48 | radar1_noise = 3; 49 | radar2_noise = 1; 50 | radar3_noise = 4; 51 | 52 | %真实的测量信号 53 | radar1_ture_measuremnet = sqrt((radar_1(1)-x_ture_traj).^2 + (radar_1(2)-y_ture_traj).^2 ); 54 | radar2_ture_measuremnet = sqrt((radar_2(1)-x_ture_traj).^2 + (radar_2(2)-y_ture_traj).^2 ); 55 | radar3_ture_measuremnet = sqrt((radar_3(1)-x_ture_traj).^2 + (radar_3(2)-y_ture_traj).^2 ); 56 | 57 | %加噪声的测量 58 | radar1_noise_measuremnet = radar1_ture_measuremnet + radar1_noise * randn(1,size(radar1_ture_measuremnet,2)); 59 | radar2_noise_measuremnet = radar2_ture_measuremnet + radar2_noise * randn(1,size(radar2_ture_measuremnet,2)); 60 | radar3_noise_measuremnet = radar3_ture_measuremnet + radar3_noise * randn(1,size(radar3_ture_measuremnet,2)); 61 | 62 | 63 | %% 64 | %FR结构的联邦滤波器(融合重置结构) 65 | %联邦滤波器 三个子滤波器 一个主滤波器 66 | beta_M = 0; 67 | N = 3; 68 | beta_i = 1/N;%每个传感器分配的权值是一样的 69 | 70 | Q_process_noise_system = diag([2 2]);%系统的过程噪声，题目的要求 71 | P_cov_system = diag([1000 1000 ]);%初始的系统协方差,随便给定的 72 | X_init_state = [0;500];%初始目标的位置 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%1.信息分配与重置%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | 75 | Q_process_noise_radar = 1/(beta_i) * Q_process_noise_system; 76 | 77 | system_init_pos = X_init_state; 78 | object1_init_pos = X_init_state; 79 | object2_init_pos = X_init_state; 80 | object3_init_pos = X_init_state; 81 | 82 | object1_correct_pos = X_init_state; 83 | object2_correct_pos = X_init_state; 84 | object3_correct_pos = X_init_state; 85 | fusion_pos = X_init_state; 86 | 87 | object1_init_cov = 1/(beta_i) * P_cov_system; 88 | object2_init_cov = 1/(beta_i) * P_cov_system; 89 | object3_init_cov = 1/(beta_i) * P_cov_system; 90 | 91 | 92 | 93 | for i = 1:size(radar1_ture_measuremnet,2)-1 %第一个观测值扔掉 94 | 95 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%2.传感器时间量测更新%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 96 | 97 | %由于是匀速圆周运动，所以可以用旋转矩阵来做预测 R = [cos(theta) -sin(theta) ; sin(theta) cos(theta)] 98 | %由于旋转矩阵本身的方向是逆时针方向的，而本题物体的运动是瞬时间的，所以需要用到旋转矩阵的逆 = 旋转矩阵的转置 99 | theta = w * Observation_time * 1; 100 | R = [cos(theta) -sin(theta); 101 | sin(theta) cos(theta)]'; %对于每个传感器都是一样的 102 | %% 103 | %传感器1 104 | %radar1的卡尔曼子滤波器 105 | %预测 106 | F = R; 107 | object1_estimate_pos(:,i+1) = F * object1_init_pos; 108 | P_object1_estimate = F * object1_init_cov * F' + Q_process_noise_radar; 109 | 110 | %更新 111 | %因为观测并不是线性的，所以这里需要用到拓展卡尔曼滤波，对观测方程进行求导 112 | %观测矩阵也是不断变化的，因为是在均值处进行展开 113 | dec = sqrt((radar_1(1)-object1_estimate_pos(1,i+1)).^2 + (radar_1(2)-object1_estimate_pos(2,i+1)).^2); 114 | H = [ -(radar_1(1)-object1_estimate_pos(1,i+1))/dec -(radar_1(2)-object1_estimate_pos(2,i+1))/dec]; 115 | 116 | K = P_object1_estimate * H' * inv(H * P_object1_estimate * H' + radar1_noise) ; 117 | object1_correct_pos(:,i+1) = object1_estimate_pos(:,i+1) + K * ( radar1_noise_measuremnet(i+1) - sqrt((radar_1(1)-object1_estimate_pos(1,i+1)).^2 + (radar_1(2)-object1_estimate_pos(2,i+1)).^2)); 118 | inform_object1_correct = inv((eye(2) - K * H)*P_object1_estimate); 119 | 120 | 121 | %radar2的卡尔曼子滤波器 122 | 123 | object2_estimate_pos(:,i+1) = F * object2_init_pos; 124 | P_object2_estimate = F * object2_init_cov * F' + Q_process_noise_radar; 125 | 126 | %更新 127 | 128 | dec = sqrt((radar_2(1)-object2_estimate_pos(1,i+1)).^2 + (radar_2(2)-object2_estimate_pos(2,i+1)).^2); 129 | H = [ -(radar_2(1)-object2_estimate_pos(1,i+1))/dec -(radar_2(2)-object2_estimate_pos(2,i+1))/dec]; 130 | 131 | 132 | K = P_object2_estimate * H' * inv(H * P_object2_estimate * H' + radar2_noise) ; 133 | object2_correct_pos(:,i+1) = object2_estimate_pos(:,i+1) + K * ( radar2_noise_measuremnet(i+1) - sqrt((radar_2(1)-object2_estimate_pos(1,i+1)).^2 + (radar_2(2)-object2_estimate_pos(2,i+1)).^2)); 134 | inform_object2_correct = inv((eye(2) - K * H)*P_object2_estimate); 135 | 136 | %% 137 | %radar3的卡尔曼子滤波器 138 | %object3_init_pos 139 | object3_estimate_pos(:,i+1) = F * object3_init_pos; 140 | P_object3_estimate = F * object3_init_cov * F' + Q_process_noise_radar; 141 | 142 | %更新 143 | 144 | dec = sqrt((radar_3(1)-object3_estimate_pos(1,i+1)).^2 + (radar_3(2)-object3_estimate_pos(2,i+1)).^2); 145 | H = [ -(radar_3(1)-object3_estimate_pos(1,i+1))/dec -(radar_3(2)-object3_estimate_pos(2,i+1))/dec]; 146 | 147 | 148 | 149 | K = P_object3_estimate * H' * inv(H * P_object3_estimate * H' + radar3_noise) ; 150 | object3_correct_pos(:,i+1) = object3_estimate_pos(:,i+1) + K * ( radar3_noise_measuremnet(i+1) - sqrt( (radar_3(1)-object3_estimate_pos(1,i+1)).^2 + (radar_3(2)-object3_estimate_pos(2,i+1)).^2) ); 151 | inform_object3_correct = inv((eye(2) - K * H)*P_object3_estimate); 152 | 153 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%3.主滤波器时间更新%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 154 | 155 | %主滤波器 有3种模式，NR,ZR,FS 156 | 157 | system_estimate_pos = F * system_init_pos; 158 | system_estimate_cov = F * P_cov_system * F'; 159 | 160 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%4.最后的融合处理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 161 | fusion_cov = inv( inform_object1_correct + inform_object2_correct + inform_object3_correct + inv(system_estimate_cov)); 162 | fusion_pos(:,i+1) = fusion_cov* (inform_object1_correct *object1_correct_pos(:,i+1) + inform_object2_correct * object2_correct_pos(:,i+1) + inform_object3_correct *object3_correct_pos(:,i+1) + inv(system_estimate_cov) * system_estimate_pos ); 163 | 164 | 165 | object1_init_pos = fusion_pos(:,i+1); 166 | object1_init_cov = 1/(beta_i) * fusion_cov; 167 | object2_init_pos = fusion_pos(:,i+1); 168 | object2_init_cov = 1/(beta_i) * fusion_cov; 169 | object3_init_pos = fusion_pos(:,i+1); 170 | object3_init_cov = 1/(beta_i) * fusion_cov; 171 | 172 | system_init_pos = fusion_pos(:,i+1); 173 | P_cov_system = fusion_cov ; 174 | 175 | end 176 | 177 | plot(x,[y;-y],'LineWidth',2); 178 | grid on;axis equal;title('质点轨迹','FontSize',16) 179 | xlabel('x/m','FontSize',16) 180 | ylabel('y/m','FontSize',16) 181 | xlim([-2000 2000]) 182 | ylim([-2000 2000]) 183 | text(750,750,'o','color','g') 184 | text(750,750,'雷达1','FontSize',10) 185 | text(-750,750,'o','color','g') 186 | text(-750,750,'雷达2','FontSize',10) 187 | text(0,-1000,'o','color','g') 188 | text(0,-1000,'雷达3','FontSize',10) 189 | hold on 190 | comet(fusion_pos(1,:),fusion_pos(2,:)) 191 | 192 | for i=1:size(radar1_ture_measuremnet,2)-1 193 | % Err_S(i)=Dist(ture_traj(:,i),s_traj(:,i));%滤波前的误差 194 | Err_FF(i)=Dist(fusion_pos(:,i),ture_traj(:,i));%滤波前的误差 195 | % Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差 196 | end 197 | % mean_S=mean(Err_S); 198 | mean_FF=mean(Err_FF); 199 | % mean_UKF=mean(Err_UKF); 200 | % mean_PF=mean(Err_PF); 201 | t=1:size(radar1_ture_measuremnet,2)-1; 202 | figure 203 | hold on;box on; 204 | % plot(t,Err_Obs,'-'); 205 | % plot(t,Err_UKF,'--'); 206 | % plot(t,Err_S,'-.'); 207 | plot(t,Err_FF,'-.'); 208 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 209 | legend(sprintf('FF滤波后误差%.03f',mean_FF)); 210 | xlabel('观测时间/s'); 211 | ylabel('误差值'); 212 | 213 | function d=Dist(X1,X2) 214 | if length(X2)<=2 215 | d=sqrt((X1(1,1)-X2(1,1))^2+(X1(2,1)-X2(2,1))^2); 216 | else 217 | % d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2); 218 | end 219 | end -------------------------------------------------------------------------------- /3/code_0/BP_standard.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | %标准BP算法 3 | x=[1.24,1.27;1.36,1.74;1.38,1.64;1.38,1.82;1.38,1.90; 4 | 1.40,1.70;1.48,1.82;1.54,1.82;1.56,2.08;1.14,1.82;1.18,1.96;1.20,1.86;1.26,2.00 5 | 1.28,2.00;1.30,1.96]; 6 | y=[1 0;1 0;1 0;1 0;1 0;1 0;1 0;1 0;1 0;0 1;0 1;0 1;0 1;0 1;0 1]; %初始数据 7 | 8 | [v,r,w,h,y_hat]=standard_BP(x,y,1,1e6); 9 | 10 | V=[-2 2 -2 2]; 11 | P=[0.1 0.7 0.8 0.8 1.0 0.3 0.0 -0.3 -0.5 -1.5;1.2 1.8 1.6 0.6 0.8 0.5 0.2 0.8 -1.5 -1.3]; 12 | T=[1 1 1 0 0 1 1 1 0 0;0 0 0 0 0 1 1 1 1 1]; 13 | %取一数组限制坐标值的大小 14 | plotpv(P,T,V); 15 | %该函数用于在感知器向量图中绘制其要分类的矢量图 16 | axis('equal'), 17 | %令横坐标和纵坐标单位距离相等 18 | title('Input Vector Graph'), 19 | %命名图的标题 20 | xlabel('p1'), 21 | %命名横轴坐标 22 | ylabel('p2'), 23 | %命名纵轴坐标 24 | plotpc(w',h'); 25 | %该函数用于绘制感知器的输入矢量的目标向量，即在plotpv中把最终的分界线画出来 26 | 27 | x_k=[1.24 1.80;1.28 1.84;1.40 2.04]; 28 | 29 | function [v,r,w,h,y_hat]=standard_BP(x0,y0,q,N) %q为隐层单元数目,N,迭代次数 30 | 31 | %初始化连边权值和阀值 32 | L=size(y0,2); %输出单元数目 33 | [m,n]=size(x0); %获取数据的维度 34 | v=rand(n,q); %初始化输入层到隐层的权值 35 | r=rand(1,q); %初始化隐层的阀值 36 | w=rand(q,L); %初始化隐层到输出层的权值 37 | h=rand(1,L); %初始化输出层的阀值 38 | k=0.05; %学习率 39 | E=1; 40 | 41 | iter=1; 42 | a=1; 43 | 44 | while iter隐层,各个隐层单元具有的权值 49 | b=fc_sigmod(A-r); %经过隐层的激活函数的输出 50 | 51 | B=b*w; %隐层->输出层,各个输出层单元具有的权值 52 | y_hat(a,:)=fc_sigmod(B-h); %经过输出层的激活函数的输出 53 | 54 | E=0.5*sum((y_hat(a,:)-y).^2); %求均方误差 55 | 56 | %以下对各个系数进行调整 57 | g=y_hat(a,:).*(1-y_hat(a,:)).*(y-y_hat(a,:)); 58 | e=b.*(1-b).*(w*g')'; 59 | 60 | for i=1:n 61 | for j=1:q 62 | v(i,j)=v(i,j)+k*e(j)*x(i); %输入层->隐层的权值更新 63 | end 64 | end 65 | r=r-k*e; %隐层的阀值更新 66 | 67 | for i=1:q 68 | for j=1:L 69 | w(i,j)=w(i,j)+k*g(j)*b(i); %隐层->输出层的权值更新 70 | end 71 | end 72 | h=h-k*g; 73 | 74 | if a>=m 75 | a=a-m; 76 | end 77 | a=a+1; 78 | iter=iter+1; 79 | end 80 | 81 | disp('输入层到隐层的权值为');v 82 | disp('隐层的阀值为');r 83 | disp('隐层到输出层的权值为');w 84 | disp('输出层的阀值为');h 85 | 86 | function y=fc_sigmod(x) 87 | y=1./(1+exp(-x)); 88 | end 89 | end 90 | -------------------------------------------------------------------------------- /3/code_0/Copy_of_BP_standard.m: -------------------------------------------------------------------------------- 1 | P=[0.1 0.7 0.8 0.8 1.0 0.3 0.0 -0.3 -0.5 -1.5;1.2 1.8 1.6 0.6 0.8 0.5 0.2 0.8 -1.5 -1.3]; 2 | T=[1 1 1 0 0 1 1 1 0 0;0 0 0 0 0 1 1 1 1 1]; 3 | [R,Q]=size(P); 4 | [S,Q]=size(T); 5 | net=newp(minmax(P),S); 6 | %建立一个有S个输出的感知器网络 7 | [W0]=rands(S,R); 8 | [B0]=rands(S,1); 9 | %初始化给权值，偏差 10 | net.iw{1,1}=W0; 11 | net.b{1}=B0; 12 | %给权值，偏差初始化赋值 13 | net.trainParam.epochs=20; 14 | %定义最大循环次数 15 | net=train(net,P,T); 16 | %进行神经网络的训练 17 | V=[-2 2 -2 2]; 18 | %取一数组限制坐标值的大小 19 | plotpv(P,T,V); 20 | %该函数用于在感知器向量图中绘制其要分类的矢量图 21 | axis('equal'), 22 | %令横坐标和纵坐标单位距离相等 23 | title('Input Vector Graph'), 24 | %命名图的标题 25 | xlabel('p1'), 26 | %命名横轴坐标 27 | ylabel('p2'), 28 | %命名纵轴坐标 29 | plotpc(net.iw{1},net.b{1}); 30 | %该函数用于绘制感知器的输入矢量的目标向量，即在plotpv中把最终的分界线画出来 31 | -------------------------------------------------------------------------------- /3/code_1/BP.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | % 观测点数 3 | N = 500; 4 | x = linspace(-2*pi, 2*pi,N ); 5 | % 信号 6 | y = 3 * sin(x)+2*cos(x.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(N , 1).*y'; 9 | v2 = 0.05*randn(N , 1); 10 | v=v1+v2; 11 | z=y'+v; 12 | d=y'; 13 | P=x; 14 | T=z'; 15 | 16 | %%% 画图 17 | % plot(x,y); 18 | % hold on; 19 | % plot(x,T); 20 | % axis([-2*pi 2*pi -6 6]); 21 | % legend('y-x曲线','z-x曲线'); 22 | % xlabel('横坐标 x'); 23 | % ylabel('纵坐标 y'); 24 | % 25 | % figure; 26 | % plot(x,v'); 27 | % axis([-2*pi 2*pi -0.7 0.7]); 28 | % legend('v-x曲线'); 29 | % xlabel('横坐标 x'); 30 | % ylabel('纵坐标 y'); 31 | 32 | % 数据集设置 33 | [a,b]=dividerand([P;T],0.8,0.2); %训练集+验证集 80%，测试集20% 34 | trainp=a(1,:); 35 | traint=a(2,:); 36 | testp=b(1,:); 37 | testt=b(2,:); 38 | 39 | %BP网络创建 40 | 41 | net=newff(P,T,[50]); 42 | 43 | 44 | % 指定训练参数 45 | net.trainFcn='trainlm'; 46 | 47 | %BP网络参数的设置 48 | net.trainParam 49 | net.trainParam.epochs=1000;%训练次数设置 50 | net.trainParam.goal=1e-7;%训练目标设置 51 | net.trainParam.lr=0.01;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 52 | net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 53 | net.trainParam.show=25;%显示的间隔次数 54 | net.divideParam.trainRatio=6/8; %训练集 60% 55 | net.divideParam.valRatio=2/8;%验证集 20% 56 | net.divideParam.testRatio=0;%验证集 20% 57 | 58 | 59 | %RBF网络创建与训练 60 | % net_rbf=newrb(trainp,traint,1); 61 | 62 | %RBF参数设置 63 | % net_rbf.trainParam 64 | % net_rbf.trainParam.epochs=10000;%训练次数设置 65 | 66 | 67 | %训练网络 68 | tic; 69 | [net,tr]=train(net,trainp,traint); 70 | toc; 71 | % tic; 72 | % %RBF网络创建 73 | % net=newrb(P,T,0,1,300,25); 74 | % toc 75 | 76 | % [net_rbf,tr_rbf]=train(net_rbf,trainp,traint); 77 | % 测试结果 78 | [output]=sim(net,testp);%训练的数据，根据BP得到的结果 79 | % [output_rbf] = sim(net_rbf,testp); 80 | test_mse = mse(output-testt); 81 | 82 | figure; 83 | plot(x,d); 84 | hold on; 85 | plot(trainp,traint); 86 | plot(testp,output,"o"); 87 | % plot(testp,testt,"x"); 88 | title("BF"); 89 | legend('理论输出','实际输出','预测点'); 90 | xlabel('横坐标 T/s'); 91 | ylabel('纵坐标 X/m'); 92 | 93 | % figure; 94 | % plot(x,d); 95 | % hold on; 96 | % plot(trainp,traint); 97 | % plot(testp,output_rbf,"o"); 98 | % % plot(testp,testt,"x"); 99 | % title("RBF"); 100 | % legend('理论输出','实际输出','预测点'); 101 | % xlabel('横坐标 T/s'); 102 | % ylabel('纵坐标 X/m'); 103 | 104 | % %%%%%%%%%%% 105 | % %画图 106 | % figure 107 | % hold on ;box on 108 | % t=(0:1:N-1); 109 | % plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 110 | % % plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 111 | % plot(t,Xukf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 112 | % % plot(X(1,:),X(3,:),'-k.'); 113 | % % plot(Xukf(1,:),Xukf(3,:),'-r+'); 114 | % legend('真实轨迹','UKF轨迹'); 115 | % xlabel('横坐标 T/s'); 116 | % ylabel('纵坐标 X/m'); 117 | % 118 | % figure 119 | % hold on;box on; 120 | % plot(t,Err_Obs,'-'); 121 | % plot(t,Err_UKF,'--'); 122 | % % plot(t,Err_EKF1,'-.'); 123 | % % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 124 | % legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF)); 125 | % xlabel('观测时间/s'); 126 | % ylabel('误差值'); 127 | % 128 | % %%%%%%%%%%%%% 129 | % %子函数 130 | % % 计算欧氏距离子函数 131 | % function dist=RMS(X1,X2) 132 | % if length(X2)<=2 133 | % dist=sqrt((X1(1)-X2(1))^2); 134 | % else 135 | % dist=sqrt((X1(1)-X2(1))^2); 136 | % end 137 | % end 138 | -------------------------------------------------------------------------------- /3/code_1/BP_RBF.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | % 观测点数 3 | N = 500; 4 | x = linspace(-2*pi, 2*pi,N ); 5 | % 信号 6 | y = 3 * sin(x)+2*cos(x.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(N , 1).*y'; 9 | v2 = 0.05*randn(N , 1); 10 | v=v1+v2; 11 | z=y'+v; 12 | d=y'; 13 | P=x; 14 | T=z'; 15 | 16 | %%% 画图 17 | % plot(x,y); 18 | % hold on; 19 | % plot(x,T); 20 | % axis([-2*pi 2*pi -6 6]); 21 | % legend('y-x曲线','z-x曲线'); 22 | % xlabel('横坐标 x'); 23 | % ylabel('纵坐标 y'); 24 | % 25 | % figure; 26 | % plot(x,v'); 27 | % axis([-2*pi 2*pi -0.7 0.7]); 28 | % legend('v-x曲线'); 29 | % xlabel('横坐标 x'); 30 | % ylabel('纵坐标 y'); 31 | 32 | % 数据集设置 33 | [a,b]=dividerand([P;T],0.8,0.2); %训练集+验证集 80%，测试集20% 34 | trainp=a(1,:); 35 | traint=a(2,:); 36 | testp=b(1,:); 37 | testt=b(2,:); 38 | 39 | %BP网络创建 40 | 41 | net=newff(P,T,[50]); 42 | 43 | 44 | % 指定训练参数 45 | net.trainFcn='trainlm'; 46 | 47 | %BP网络参数的设置 48 | net.trainParam 49 | net.trainParam.epochs=300;%训练次数设置 50 | net.trainParam.goal=1e-7;%训练目标设置 51 | net.trainParam.lr=0.001;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 52 | net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 53 | net.trainParam.show=25;%显示的间隔次数 54 | net.divideParam.trainRatio=8/8; %训练集 60% 55 | net.divideParam.valRatio=0;%验证集 20% 56 | net.divideParam.testRatio=0;%验证集 20% 57 | 58 | 59 | %RBF网络创建与训练 60 | % net_rbf=newrb(trainp,traint,1); 61 | 62 | %RBF参数设置 63 | % net_rbf.trainParam 64 | % net_rbf.trainParam.epochs=10000;%训练次数设置 65 | 66 | 67 | %训练网络 68 | start1=tic; 69 | % [net,tr]=train(net,trainp,traint); 70 | % toc; 71 | time1=toc(start1); 72 | 73 | start2=tic; 74 | %RBF网络创建及训练 75 | net_rbf=newrb(trainp,traint,1e-7,0.8,100,25); 76 | [net_rbf,tr]=train(net_rbf,trainp,traint); 77 | time2=toc(start2); 78 | 79 | % [net_rbf,tr_rbf]=train(net_rbf,trainp,traint); 80 | % 测试结果 81 | [output]=sim(net,testp);%训练的数据，根据BP得到的结果 82 | [output_rbf] = sim(net_rbf,testp); 83 | test_mse = mse(output-testt); 84 | test_mse_rbf = mse(output_rbf-testt); 85 | 86 | figure; 87 | 88 | plot(x,d); 89 | axis([-2*pi 2*pi -6 6]); 90 | hold on; 91 | plot(trainp,traint); 92 | plot(testp,output,"o"); 93 | plot(testp,output_rbf,"x"); 94 | title("BF网络与RBF的对比图"); 95 | % legend('理论输出','实际输出','BP预测点','RBF预测点'); 96 | legend('理论输出','实际输出',sprintf('BP预测点%.03f 用时%.03f',test_mse,time1),sprintf('RBF预测点%.03f 用时%.03f',test_mse_rbf,time2)); 97 | xlabel('横坐标 X'); 98 | ylabel('纵坐标 Y'); 99 | 100 | % figure; 101 | % plot(x,d); 102 | % hold on; 103 | % plot(trainp,traint); 104 | % plot(testp,output_rbf,"o"); 105 | % % plot(testp,testt,"x"); 106 | % title("RBF"); 107 | % legend('理论输出','实际输出','预测点'); 108 | % xlabel('横坐标 T/s'); 109 | % ylabel('纵坐标 X/m'); 110 | 111 | % %%%%%%%%%%% 112 | % %画图 113 | % figure 114 | % hold on ;box on 115 | % t=(0:1:N-1); 116 | % plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 117 | % % plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 118 | % plot(t,Xukf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 119 | % % plot(X(1,:),X(3,:),'-k.'); 120 | % % plot(Xukf(1,:),Xukf(3,:),'-r+'); 121 | % legend('真实轨迹','UKF轨迹'); 122 | % xlabel('横坐标 T/s'); 123 | % ylabel('纵坐标 X/m'); 124 | % 125 | % figure 126 | % hold on;box on; 127 | % plot(t,Err_Obs,'-'); 128 | % plot(t,Err_UKF,'--'); 129 | % % plot(t,Err_EKF1,'-.'); 130 | % % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 131 | % legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF)); 132 | % xlabel('观测时间/s'); 133 | % ylabel('误差值'); 134 | % 135 | % %%%%%%%%%%%%% 136 | % %子函数 137 | % % 计算欧氏距离子函数 138 | % function dist=RMS(X1,X2) 139 | % if length(X2)<=2 140 | % dist=sqrt((X1(1)-X2(1))^2); 141 | % else 142 | % dist=sqrt((X1(1)-X2(1))^2); 143 | % end 144 | % end 145 | -------------------------------------------------------------------------------- /3/code_1/RBF.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | % 观测点数 3 | N = 500; 4 | x = linspace(-2*pi, 2*pi,N ); 5 | % 信号 6 | y = 3 * sin(x)+2*cos(x.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(N , 1).*y'; 9 | v2 = 0.05*randn(N , 1); 10 | v=v1+v2; 11 | z=y'+v; 12 | d=y'; 13 | P=x; 14 | T=z'; 15 | 16 | %%% 画图 17 | % plot(x,y); 18 | % hold on; 19 | % plot(x,T); 20 | % axis([-2*pi 2*pi -6 6]); 21 | % legend('y-x曲线','z-x曲线'); 22 | % xlabel('横坐标 x'); 23 | % ylabel('纵坐标 y'); 24 | % 25 | % figure; 26 | % plot(x,v'); 27 | % axis([-2*pi 2*pi -0.7 0.7]); 28 | % legend('v-x曲线'); 29 | % xlabel('横坐标 x'); 30 | % ylabel('纵坐标 y'); 31 | 32 | % 数据集设置 33 | [a,b]=dividerand([P;T],0.8,0.2); %训练集+验证集 80%，测试集20% 34 | trainp=a(1,:); 35 | traint=a(2,:); 36 | testp=b(1,:); 37 | testt=b(2,:); 38 | 39 | %RBF网络创建 40 | net=newrb(trainp,traint,10e-7,0.8,50,25); 41 | 42 | % 指定训练参数 43 | net.trainFcn='trainlm'; 44 | 45 | %BP网络参数的设置 46 | % net.trainParam.epochs=1000; %训练次数设置 47 | % net.trainParam.goal=1e-7;%训练目标设置 48 | % net.trainParam.lr=0.01;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 49 | % net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 50 | % net.trainParam.show=25;%显示的间隔次数 51 | % net.divideParam.trainRatio=6/8; %训练集 60% 52 | % net.divideParam.valRatio=2/8;%验证集 20% 53 | 54 | 55 | %RBF网络创建与训练 56 | % net_rbf=newhop(trainp,traint,1); 57 | 58 | %RBF参数设置 59 | % net_rbf.trainParam 60 | % net_rbf.trainParam.epochs=10000;%训练次数设置 61 | 62 | 63 | %训练网络 64 | [net,tr]=train(net,trainp,traint); 65 | 66 | % 测试结果 67 | [output]=sim(net,testp);%训练的数据，根据BP得到的结果 68 | test_mse = mse(output-testt); 69 | 70 | figure; 71 | plot(x,d); 72 | hold on; 73 | plot(trainp,traint); 74 | plot(testp,output,"o"); 75 | % plot(testp,testt,"x"); 76 | title("RBF"); 77 | legend('理论输出','实际输出','预测点'); 78 | xlabel('横坐标 T/s'); 79 | ylabel('纵坐标 X/m'); 80 | 81 | 82 | -------------------------------------------------------------------------------- /3/code_1/example.m: -------------------------------------------------------------------------------- 1 | clc 2 | 3 | clear all 4 | 5 | close all 6 | 7 | %bp 神经网络的预测代码 8 | 9 | %载入输出和输入数据 10 | 11 | % 观测点数 12 | N = 100; 13 | x = linspace(-2*pi, 2*pi,N ); 14 | % 信号 15 | y = 3 * sin(x)+2*cos(x.^2); 16 | % 噪声（方差1.25） 17 | v1 = 0.05*randn(N , 1).*y'; 18 | v2 = 0.05*randn(N , 1); 19 | v=v1+v2; 20 | z=y'+v; 21 | 22 | 23 | %赋值给输出p和输入t 24 | 25 | p=x'; 26 | 27 | t=z; 28 | 29 | %数据的归一化处理，利用mapminmax函数，使数值归一化到[-1.1]之间 30 | 31 | %该函数使用方法如下：[y,ps] =mapminmax(x,ymin,ymax)，x需归化的数据输入， 32 | 33 | %ymin，ymax为需归化到的范围，不填默认为归化到[-1,1] 34 | 35 | %返回归化后的值y，以及参数ps，ps在结果反归一化中，需要调用 36 | 37 | [p1,ps]=mapminmax(p); 38 | 39 | [t1,ts]=mapminmax(t); 40 | 41 | %确定训练数据，测试数据,一般是随机的从样本中选取70%的数据作为训练数据 42 | 43 | %15%的数据作为测试数据，一般是使用函数dividerand，其一般的使用方法如下： 44 | 45 | %[trainInd,valInd,testInd] = dividerand(Q,trainRatio,valRatio,testRatio) 46 | 47 | [trainsample.p,valsample.p,testsample.p] =dividerand(p,0.7,0.15,0.15); 48 | 49 | [trainsample.t,valsample.t,testsample.t] =dividerand(t,0.7,0.15,0.15); 50 | 51 | %建立反向传播算法的BP神经网络，使用newff函数，其一般的使用方法如下 52 | 53 | %net = newff(minmax(p),[隐层的神经元的个数，输出层的神经元的个数],{隐层神经元的传输函数，输出层的传输函数｝,'反向传播的训练函数'),其中p为输入数据，t为输出数据 54 | 55 | %tf为神经网络的传输函数，默认为'tansig'函数为隐层的传输函数， 56 | 57 | %purelin函数为输出层的传输函数 58 | 59 | %一般在这里还有其他的传输的函数一般的如下，如果预测出来的效果不是很好，可以调节 60 | 61 | %TF1 = 'tansig';TF2 = 'logsig'; 62 | 63 | %TF1 = 'logsig';TF2 = 'purelin'; 64 | 65 | %TF1 = 'logsig';TF2 = 'logsig'; 66 | 67 | %TF1 = 'purelin';TF2 = 'purelin'; 68 | 69 | TF1='tansig';TF2='purelin'; 70 | 71 | net=newff(minmax(p),[10,1],{TF1 TF2},'traingdm');%网络创建 72 | 73 | %网络参数的设置 74 | 75 | net.trainParam.epochs=10000;%训练次数设置 76 | 77 | net.trainParam.goal=1e-7;%训练目标设置 78 | 79 | net.trainParam.lr=0.01;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 80 | 81 | net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 82 | 83 | net.trainParam.show=25;%显示的间隔次数 84 | 85 | % 指定训练参数 86 | 87 | % net.trainFcn = 'traingd'; % 梯度下降算法 88 | 89 | % net.trainFcn = 'traingdm'; % 动量梯度下降算法 90 | 91 | % net.trainFcn = 'traingda'; % 变学习率梯度下降算法 92 | 93 | % net.trainFcn = 'traingdx'; % 变学习率动量梯度下降算法 94 | 95 | % (大型网络的首选算法) 96 | 97 | % net.trainFcn = 'trainrp'; % RPROP(弹性BP)算法,内存需求最小 98 | 99 | % 共轭梯度算法 100 | 101 | % net.trainFcn = 'traincgf'; %Fletcher-Reeves修正算法 102 | 103 | % net.trainFcn = 'traincgp'; %Polak-Ribiere修正算法,内存需求比Fletcher-Reeves修正算法略大 104 | 105 | % net.trainFcn = 'traincgb'; % Powell-Beal复位算法,内存需求比Polak-Ribiere修正算法略大 106 | 107 | % (大型网络的首选算法) 108 | 109 | % %net.trainFcn = 'trainscg'; % ScaledConjugate Gradient算法,内存需求与Fletcher-Reeves修正算法相同,计算量比上面三种算法都小很多 110 | 111 | % net.trainFcn = 'trainbfg'; %Quasi-Newton Algorithms - BFGS Algorithm,计算量和内存需求均比共轭梯度算法大,但收敛比较快 112 | 113 | % net.trainFcn = 'trainoss'; % OneStep Secant Algorithm,计算量和内存需求均比BFGS算法小,比共轭梯度算法略大 114 | 115 | % (中型网络的首选算法) 116 | 117 | %net.trainFcn = 'trainlm'; %Levenberg-Marquardt算法,内存需求最大,收敛速度最快 118 | 119 | % net.trainFcn = 'trainbr'; % 贝叶斯正则化算法 120 | 121 | % 有代表性的五种算法为:'traingdx','trainrp','trainscg','trainoss', 'trainlm' 122 | 123 | %在这里一般是选取'trainlm'函数来训练，其算对对应的是Levenberg-Marquardt算法 124 | 125 | net.trainFcn='trainlm'; 126 | 127 | [net,tr]=train(net,trainsample.p,trainsample.t); 128 | 129 | %计算仿真，其一般用sim函数 130 | 131 | [normtrainoutput,trainPerf]=sim(net,trainsample.p,[],[],trainsample.t);%训练的数据，根据BP得到的结果 132 | 133 | [normvalidateoutput,validatePerf]=sim(net,valsample.p,[],[],valsample.t);%验证的数据，经BP得到的结果 134 | 135 | [normtestoutput,testPerf]=sim(net,testsample.p,[],[],testsample.t);%测试数据，经BP得到的结果 136 | 137 | %将所得的结果进行反归一化，得到其拟合的数据 138 | 139 | trainoutput=mapminmax('reverse',normtrainoutput,ts); 140 | 141 | validateoutput=mapminmax('reverse',normvalidateoutput,ts); 142 | 143 | testoutput=mapminmax('reverse',normtestoutput,ts); 144 | 145 | %正常输入的数据的反归一化的处理，得到其正式值 146 | 147 | trainvalue=mapminmax('reverse',trainsample.t,ts);%正常的验证数据 148 | 149 | validatevalue=mapminmax('reverse',valsample.t,ts);%正常的验证的数据 150 | 151 | testvalue=mapminmax('reverse',testsample.t,ts);%正常的测试数据 152 | 153 | %做预测，输入要预测的数据pnew 154 | 155 | pnew=[313,256,239]'; 156 | pnewn=mapminmax(pnew); 157 | anewn=sim(net,pnewn); 158 | anew=mapminmax('reverse',anewn,ts); 159 | %绝对误差的计算 160 | errors=trainvalue-trainoutput; 161 | %plotregression拟合图 162 | figure,plotregression(trainvalue,trainoutput) 163 | %误差图 164 | figure,plot(1:length(errors),errors,'-b') 165 | title('误差变化图') 166 | %误差值的正态性的检验 167 | figure,hist(errors);%频数直方图 168 | figure,normplot(errors);%Q-Q图 169 | [muhat,sigmahat,muci,sigmaci]=normfit(errors);%参数估计 均值,方差,均值的0.95置信区间,方差的0.95置信区间 170 | [h1,sig,ci]= ttest(errors,muhat);%假设检验 171 | figure, ploterrcorr(errors);%绘制误差的自相关图 172 | figure, parcorr(errors);%绘制偏相关图 -------------------------------------------------------------------------------- /3/code_2/BP.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | % 观测点数 3 | N = 100; 4 | x = linspace(-2*pi, 2*pi,N ); 5 | % 信号 6 | y = 3 * sin(x)+2*cos(x.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(N , 1).*y'; 9 | v2 = 0.05*randn(N , 1); 10 | v=v1+v2; 11 | z=y'+v; 12 | d=y'; 13 | P=x; 14 | T=z'; 15 | 16 | %%% 画图 17 | % plot(x,y); 18 | % hold on; 19 | % plot(x,T); 20 | % axis([-2*pi 2*pi -6 6]); 21 | % legend('y-x曲线','z-x曲线'); 22 | % xlabel('横坐标 x'); 23 | % ylabel('纵坐标 y'); 24 | % 25 | % figure; 26 | % plot(x,v'); 27 | % axis([-2*pi 2*pi -0.7 0.7]); 28 | % legend('v-x曲线'); 29 | % xlabel('横坐标 x'); 30 | % ylabel('纵坐标 y'); 31 | 32 | % 数据集设置 33 | [a,b]=dividerand([P;T],0.8,0.2); %训练集+验证集 80%，测试集20% 34 | trainp=a(1,:); 35 | traint=a(2,:); 36 | testp=b(1,:); 37 | testt=b(2,:); 38 | 39 | %BP网络创建 40 | 41 | net=newff(P,T,[50]); 42 | 43 | 44 | % 指定训练参数 45 | net.trainFcn='trainlm'; 46 | 47 | %BP网络参数的设置 48 | net.trainParam 49 | net.trainParam.epochs=1000;%训练次数设置 50 | net.trainParam.goal=1e-7;%训练目标设置 51 | net.trainParam.lr=0.01;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 52 | net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 53 | net.trainParam.show=25;%显示的间隔次数 54 | net.divideParam.trainRatio=6/8; %训练集 60% 55 | net.divideParam.valRatio=2/8;%验证集 20% 56 | net.divideParam.testRatio=0;%验证集 20% 57 | 58 | 59 | %RBF网络创建与训练 60 | % net_rbf=newrb(trainp,traint,1); 61 | 62 | %RBF参数设置 63 | % net_rbf.trainParam 64 | % net_rbf.trainParam.epochs=10000;%训练次数设置 65 | 66 | 67 | %训练网络 68 | tic; 69 | [net,tr]=train(net,trainp,traint); 70 | toc; 71 | % tic; 72 | % %RBF网络创建 73 | % net=newrb(P,T,0,1,300,25); 74 | % toc 75 | 76 | % [net_rbf,tr_rbf]=train(net_rbf,trainp,traint); 77 | % 测试结果 78 | [output]=sim(net,trainp);%训练的数据，根据BP得到的结果 79 | % [output_rbf] = sim(net_rbf,testp); 80 | test_mse = mse(output-traint); 81 | 82 | figure; 83 | plot(x,d); 84 | hold on; 85 | plot(trainp,traint); 86 | plot(trainp,output); 87 | % plot(testp,testt,"x"); 88 | title("BF"); 89 | legend('理论输出','实际输出','滤波后'); 90 | xlabel('横坐标 T/s'); 91 | ylabel('纵坐标 X/m'); 92 | 93 | % figure; 94 | % plot(x,d); 95 | % hold on; 96 | % plot(trainp,traint); 97 | % plot(testp,output_rbf,"o"); 98 | % % plot(testp,testt,"x"); 99 | % title("RBF"); 100 | % legend('理论输出','实际输出','预测点'); 101 | % xlabel('横坐标 T/s'); 102 | % ylabel('纵坐标 X/m'); 103 | 104 | % %%%%%%%%%%% 105 | % %画图 106 | % figure 107 | % hold on ;box on 108 | % t=(0:1:N-1); 109 | % plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹 110 | % % plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹 111 | % plot(t,Xukf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹 112 | % % plot(X(1,:),X(3,:),'-k.'); 113 | % % plot(Xukf(1,:),Xukf(3,:),'-r+'); 114 | % legend('真实轨迹','UKF轨迹'); 115 | % xlabel('横坐标 T/s'); 116 | % ylabel('纵坐标 X/m'); 117 | % 118 | % figure 119 | % hold on;box on; 120 | % plot(t,Err_Obs,'-'); 121 | % plot(t,Err_UKF,'--'); 122 | % % plot(t,Err_EKF1,'-.'); 123 | % % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 124 | % legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF)); 125 | % xlabel('观测时间/s'); 126 | % ylabel('误差值'); 127 | % 128 | % %%%%%%%%%%%%% 129 | % %子函数 130 | % % 计算欧氏距离子函数 131 | % function dist=RMS(X1,X2) 132 | % if length(X2)<=2 133 | % dist=sqrt((X1(1)-X2(1))^2); 134 | % else 135 | % dist=sqrt((X1(1)-X2(1))^2); 136 | % end 137 | % end 138 | -------------------------------------------------------------------------------- /3/code_2/EKF_standard.m: -------------------------------------------------------------------------------- 1 | clear;clc; close all; 2 | N = 100; 3 | Q = 5;w=sqrt(Q)*randn(1,N); 4 | R = 5;v=sqrt(R)*randn(1,N); 5 | P =eye(1); 6 | x=zeros(1,N); 7 | s=zeros(1,N); 8 | Xekf=zeros(1,N); 9 | Xekf_1=zeros(1,N); 10 | x(1,1)=0.1; 11 | s(1,1)=0.1; 12 | Xekf(1,1)=x(1,1); 13 | Xekf_1(1,1)=x(1,1); 14 | z=zeros(1,N); 15 | % z(1)=x(1,1)^2/20+v(1); 16 | z(1)=x(1,1)+v(1); 17 | zpre=zeros(1,N); 18 | zpre(1,1)=z(1); 19 | 20 | 21 | for k = 2 : N 22 | % 模拟系统 23 | s(:,k) = 0.5 * s(:,k-1) + (2.5 * s(:,k-1) / (1 + s(:,k-1).^2)) + 8 * cos(0.4*(k-1))+4*sin((k-1)^2); 24 | x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(0.4*(k-1)) +4*sin((k-1)^2)+ w(k-1); 25 | 26 | % z(k) = x(:,k).^2 / 20 + v(k); 27 | z(k) = x(:,k)+ v(k); 28 | end 29 | 30 | %%% 设置网络 31 | 32 | 33 | % net=newff(trainp,traint,[20]); 34 | % % 指定训练参数 35 | % net.trainFcn='trainlm'; 36 | % net.trainParam.goal=1e-7;%训练目标设置 37 | % net.trainParam.lr=0.01;%学习率设置,应设置为较少值，太大虽然会在开始加快收敛速度，但临近最佳点时，会产生动荡，而致使无法收敛 38 | % net.trainParam.mc=0.9;%动量因子的设置，默认为0.9 39 | 40 | K=zeros(1,N); 41 | % 广义卡尔曼滤波 42 | for k = 2 : N 43 | %状态预测 44 | Xpre = 0.5*Xekf(:,k-1)+ 2.5*Xekf(:,k-1)/(1+Xekf(:,k-1).^2) + 8 * cos(0.4*(k-1))+4*sin((k-1)^2); 45 | 46 | F = 0.5 + 2.5 * (1-Xekf.^2)/((1+Xekf.^2).^2); 47 | % H = Xpre/10; 48 | H = 1; 49 | 50 | %均方差预测方程 51 | PP=F*P*F'+Q; 52 | if k~=2 53 | %%% 加载训练数据 54 | trainp=[trainp,[Xekf(k-1)]]; 55 | traint=[traint,Kk]; 56 | %%% 训练 57 | net=newff(trainp,traint,[30]); 58 | [net,tr]=train(net,trainp,traint); 59 | [outputK]=sim(net,[Xpre]);%训练的数据，根据BP得到的结果 60 | else 61 | trainp=ones(1,1); 62 | traint=ones(1,1); 63 | end 64 | 65 | %卡尔曼增益 66 | if k~=2 67 | Kk=0.5*PP*H'*(H*PP*H'+R)^(-1)+0.5*outputK; 68 | else 69 | Kk=PP*H'*(H*PP*H'+R)^(-1); 70 | end 71 | K(1,k)=Kk; 72 | 73 | %状态更新 74 | % zpre =Xpre.^2/20; 75 | zpre =Xpre; 76 | Xekf(k)=Xpre+Kk*(z(k)-zpre); 77 | 78 | %均方差更新 79 | P=PP-Kk*H*PP; 80 | end 81 | 82 | % 广义卡尔曼滤波 原始方法 83 | for k = 2 : N 84 | %状态预测 85 | Xpre = 0.5*Xekf_1(:,k-1)+ 2.5*Xekf_1(:,k-1)/(1+Xekf_1(:,k-1).^2) + 8 * cos(0.4*(k-1))+4*sin((k-1)^2); 86 | 87 | F = 0.5 + 2.5 * (1-Xekf_1.^2)/((1+Xekf_1.^2).^2); 88 | % H = Xpre/10; 89 | H = 1; 90 | 91 | %均方差预测方程 92 | PP=F*P*F'+Q; 93 | %卡尔曼增益 94 | Kk=PP*H'*(H*PP*H'+R)^(-1); 95 | %状态更新 96 | % zpre =Xpre.^2/20; 97 | zpre =Xpre; 98 | Xekf_1(k)=Xpre+Kk*(z(k)-zpre); 99 | 100 | %均方差更新 101 | P=PP-Kk*H*PP; 102 | end 103 | 104 | 105 | 106 | %误差分析 107 | for i=1:N 108 | Err_Obs(i)=RMS(x(:,i),z(:,i));%滤波前的误差 109 | Err_EKF(i)=RMS(x(:,i),Xekf(:,i));%滤波后的误差 110 | Err_EKF1(i)=RMS(x(:,i),Xekf_1(:,i));%滤波后的误差 111 | end 112 | mean_Obs=mean(Err_Obs); 113 | mean_EKF=mean(Err_EKF); 114 | mean_EKF1=mean(Err_EKF1); 115 | % t = 2 : N; 116 | % figure; 117 | % plot(t,x(1,t),'b',t,Xekf(1,t),'r*'); 118 | % legend('真实值','EKF估计值'); 119 | figure 120 | hold on;box on; 121 | t=(0:1:N-1); 122 | plot(t,s(1,:),'b','LineWidth',1);%理论轨迹 123 | plot(t,x(1,:),'--g','LineWidth',1);%实际轨迹 124 | plot(t,z(1,:),'-or','LineWidth',1);%观测轨迹 125 | plot(t,Xekf(1,:),':m','LineWidth',2);%卡尔曼滤波轨迹 126 | plot(t,Xekf_1(1,:),'-.k','LineWidth',2);%卡尔曼滤波轨迹 127 | % M=M'; 128 | % plot(M(1,:),'k','LineWidth',1);%一步预测轨迹 129 | legend('理论轨迹','实际运动轨迹','观测轨迹','扩展卡尔曼滤波+BP后轨迹','拓展卡尔曼'); 130 | xlabel('横坐标 T/s'); 131 | ylabel('纵坐标 X/m'); 132 | 133 | figure 134 | hold on;box on; 135 | plot(t,Err_Obs,'-'); 136 | plot(t,Err_EKF,'--'); 137 | plot(t,Err_EKF1,'-.'); 138 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 139 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('扩展卡尔曼滤波+BP后误差%.03f',mean_EKF),sprintf('扩展卡尔曼滤波后误差%.03f',mean_EKF1)); 140 | xlabel('观测时间/s'); 141 | ylabel('误差值'); 142 | 143 | 144 | % 计算欧氏距离子函数 145 | function dist=RMS(X1,X2) 146 | if length(X2)<=2 147 | dist=sqrt((X1(1)-X2(1))^2); 148 | else 149 | dist=sqrt((X1(1)-X2(1))^2); 150 | end 151 | end 152 | 153 | -------------------------------------------------------------------------------- /3/code_2/PF.m: -------------------------------------------------------------------------------- 1 | function Xpf=PF(Xpf,Z,Q,R,M,N) 2 | %粒子滤波初始化 3 | V = 2; %初始分布的方差 4 | x_P = []; % 粒子 5 | % 用一个高斯分布随机的产生初始的粒子 6 | for i = 1:M 7 | x_P(i) = Xpf(:,1) + sqrt(V) * randn; 8 | end 9 | 10 | %粒子滤波 11 | for t = 1:N 12 | for i = 1:M 13 | %从先验p(x(k)|x(k-1))中采样 14 | x_P_update(:,i) = gfun(x_P(:,i),t) + sqrt(Q)*randn; 15 | %计算采样粒子的值，为后面根据似然去计算权重做铺垫 16 | z_update(:,i) = x_P_update(:,i)^2/20; 17 | %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式 18 | P_w(:,i) = (1/sqrt(2*pi*R)) * exp(-(Z(:,t+1) - z_update(:,i))^2/(2*R)); 19 | end 20 | % 归一化. 21 | P_w = P_w./sum(P_w); 22 | 23 | %% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的 24 | for i = 1 : M 25 | x_P(:,i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代 26 | end % find( ,1) 返回第一个 符合前面条件的数的 下标 27 | 28 | %状态估计，重采样以后，每个粒子的权重都变成了1/N 29 | Xpf(:,t+1)=mean(x_P); 30 | 31 | 32 | end 33 | end 34 | 35 | function res=gfun(Xekf,t) 36 | res= 0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.4.*(t)); 37 | end 38 | 39 | function res=hfun(X,k) 40 | res=X^2/20; 41 | end -------------------------------------------------------------------------------- /3/code_2/UKF.m: -------------------------------------------------------------------------------- 1 | function Xukf=UKF(Xukf,Z,N,Q,R) 2 | 3 | %UKF滤波，UT变换 4 | L=1; %L个变量 5 | alpha=1; %0~1 6 | kalpha=0; 7 | belta=2; %建议为2 8 | ramda=3-L; 9 | for j=1:2*L+1 10 | Wm(j)=1/(2*(L+ramda)); 11 | Wc(j)=1/(2*(L+ramda)); 12 | end 13 | Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定 14 | Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定 15 | %%%%%%%%%%%%%%%% 16 | 17 | P0=eye(L);%协方差阵初始化 18 | for t=2:N 19 | xestimate=Xukf(:,t-1);%获取上一步的UKF点 20 | P=P0;%协方差阵 21 | 22 | %(1) 获得一组Sigma点集 23 | cho=(chol(P*(L+ramda)))'; 24 | for k=1:L 25 | xgamaP1(:,k)=xestimate+cho(:,k); 26 | xgamaP2(:,k)=xestimate-cho(:,k); 27 | end 28 | Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点 29 | 30 | %(2) 对Sigma点集进行一步预测 31 | Xsigmapre=gfun(Xsigma,t); 32 | 33 | %（3）计算加权均值 34 | Xpred=zeros(L,1); 35 | for k=1:2*L+1 36 | Xpred=Xpred+Wm(k)*Xsigmapre(:,k);%均值 37 | end 38 | %（4）计算加权方差 39 | Ppred=zeros(L,L); 40 | for k=1:2*L+1 41 | Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';%协方差矩阵 42 | end 43 | Ppred=Ppred+Q; 44 | 45 | %（5）根据预测值，再一次使用UT变换，得到新的sigma点集 46 | chor=(chol((L+ramda)*Ppred))'; 47 | for k=1:L 48 | XaugsigmaP1(:,k)=Xpred+chor(:,k); 49 | XaugsigmaP2(:,k)=Xpred-chor(:,k); 50 | end 51 | Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; 52 | 53 | %（6）观测预测 54 | for k=1:2*L+1 55 | Zsigmapre(1,k)=hfun(Xaugsigma(:,k),k); 56 | end 57 | 58 | %（7）计算观测预测加权均值 59 | Zpred=0; 60 | for k=1:2*L+1 61 | Zpred=Zpred+Wm(k)*Zsigmapre(:,k); 62 | end 63 | %（8）计算观测加权方差 64 | Pzz=0; 65 | for k=1:2*L+1 66 | Pzz=Pzz+Wc(k)*(Zsigmapre(:,k)-Zpred)*(Zsigmapre(:,k)-Zpred)'; 67 | end 68 | Pzz=Pzz+R; 69 | 70 | %（9）计算预测协方差 71 | Pxz=zeros(L,1); 72 | for k=1:2*L+1 73 | Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(:,k)-Zpred)'; 74 | end 75 | 76 | %（10）计算kalman增益 77 | K=Pxz*Pzz^-1; 78 | 79 | %（11）状态更新 80 | Xukf(:,t)=Xpred+K*(Z(t)-Zpred); 81 | 82 | %（12）方差更新 83 | P0=Ppred-K*Pzz*K'; 84 | end 85 | end 86 | 87 | function res=gfun(Xekf,k) 88 | res=0.5.*Xekf + 25.*Xekf./(1 + Xekf.^2) + 8.*cos(0.4.*(k)); 89 | end 90 | 91 | function res=hfun(X,k) 92 | res=X^2/20; 93 | end -------------------------------------------------------------------------------- /3/code_2/main_UKF_PF.m: -------------------------------------------------------------------------------- 1 | clear;clc; close all; 2 | %% initialize the variables 3 | N = 100; % 共进行75次 4 | N=N-1; 5 | M = 100; % 粒子数，越大效果越好，计算量也越大 6 | x = 0.1; % initial actual state 7 | Q = 1;w=sqrt(Q)*randn(1,N); 8 | R = 1;v=sqrt(R)*randn(1,N); 9 | 10 | 11 | X=zeros(1,N); 12 | s=zeros(1,N); 13 | Xpf=zeros(1,N); 14 | Xukf=zeros(1,N); 15 | X(1,1)=0.1; 16 | s(1,1)=0.1; 17 | Xpf(1,1)=X(1,1); 18 | Xukf(1,1)=X(1,1); 19 | Z=zeros(1,N); 20 | Z(1)=X(1,1)^2/20+v(1); 21 | 22 | for k = 2 : N+1 23 | % 模拟系统 24 | s(:,k) = gfun(s(:,k-1),k-1); 25 | X(:,k) = gfun(X(:,k-1),k-1)+ sqrt(Q)*randn; 26 | Z(:,k) =hfun(X(:,k),k)+ sqrt(R)*randn; 27 | end 28 | 29 | %粒子滤波pf 30 | tic; 31 | Xpf=PF(Xpf,Z,Q,R,M,N); 32 | toc; 33 | % tic; 34 | % Xukf=UKF(Xukf,Z,N+1,Q,R); 35 | % toc; 36 | 37 | for i=1:N+1 38 | Err_Obs(i)=RMS(X(:,i),Z(:,i));%滤波前的误差 39 | % Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差 40 | Err_PF(i)=RMS(X(:,i),Xpf(:,i));%滤波后的误差 41 | end 42 | mean_Obs=mean(Err_Obs); 43 | % mean_UKF=mean(Err_UKF); 44 | mean_PF=mean(Err_PF); 45 | 46 | %%%%%%%%%%% 47 | %画图 48 | figure 49 | hold on ;box on 50 | t=(0:1:N); 51 | plot(t,X(1,:),'-r','LineWidth',1);%实际轨迹 52 | % plot(t,Xukf(1,:),'-.k','LineWidth',1);%卡尔曼滤波轨迹 53 | plot(t,Xpf(1,:),'-.ob','LineWidth',1);%观测轨迹 54 | % plot(X(1,:),X(3,:),'-k.'); 55 | legend('真实轨迹','PF轨迹'); 56 | xlabel('横坐标 T/s'); 57 | ylabel('纵坐标 X/m'); 58 | 59 | figure 60 | hold on;box on; 61 | plot(t,Err_Obs,'-'); 62 | % plot(t,Err_UKF,'--'); 63 | plot(t,Err_PF,'-.'); 64 | % legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差'); 65 | legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('PF滤波后误差%.03f',mean_PF)); 66 | xlabel('观测时间/s'); 67 | ylabel('误差值'); 68 | 69 | %%%%%%%%%%%%% 70 | %子函数 71 | % 计算欧氏距离子函数 72 | function dist=RMS(X1,X2) 73 | if length(X2)<=2 74 | dist=sqrt((X1(1)-X2(1))^2); 75 | else 76 | dist=sqrt((X1(1)-X2(1))^2); 77 | end 78 | end 79 | function res=gfun(Xekf,t) 80 | res= 0.5*Xekf + 25*Xekf/(1 + Xekf^2) + 8*cos(0.4*(t)); 81 | end 82 | 83 | function res=hfun(X,k) 84 | res=X^2/20; 85 | end 86 | 87 | -------------------------------------------------------------------------------- /4/code/TS_model.m: -------------------------------------------------------------------------------- 1 | clear;clc; 2 | close all; 3 | %数据点个数51 4 | numpts=500; 5 | x1=linspace(-2*pi,2*pi,numpts); 6 | d=3 * sin(x1)+2*cos(x1.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(numpts , 1).*d'; 9 | v2 = sqrt(0.5)*randn(numpts , 1); 10 | v=v1+v2;%噪声信号 11 | y=d+v'; 12 | data=[x1' y']; %整个数据集 13 | [train,test]=dividerand(data',0.8,0.2); 14 | % trndata=data(1:2:numpts,:); %训练数据集 15 | % chkdata=data(2:2:numpts,:); %测试数据集 16 | trndata=train'; 17 | chkdata=test'; 18 | %训练数据和检验数据的分布曲线 19 | % plot(trndata(:,1),trndata(:,2),'o',chkdata(:,1),chkdata(:,2),'x') 20 | 21 | %建立T_S模糊模型 22 | %采用genfis1()函数直接由训练数据生成模糊推理系统 23 | nummfs=20; %隶属度函数个数 24 | mftype='gbellmf'; %隶属度函数类型 gbellmf钟形隶属度函数 25 | fismat=genfis1(trndata,nummfs,mftype); 26 | % showrule(fismat) 27 | % in = getTunableSettings(fismat) 28 | 29 | % 建立 Mamdani模糊系统 30 | % opt1 = genfisOptions('FCMClustering' , 'FISType' , 'mamdani' ); 31 | % opt1.NumClusters = 30; 32 | % fis = genfis (x1', y', opt1); 33 | % showrule(fis) 34 | dataRange = [min(data)' max(data)']; 35 | fisin = mamfis; 36 | fisin = addInput(fisin,[-2*pi,2*pi], 'Name' ,'in', 'NumMFs' ,32,'mftype','gaussmf'); 37 | fisin = addOutput(fisin,[-6.534905896158901,6.092046113952383], 'Name' ,'out', 'NumMFs' ,64,'mftype','gaussmf'); 38 | figure 39 | plotfis(fisin) 40 | 41 | % %绘制模糊推理系统的初始隶属度函数 42 | % [x,mf]=plotmf(fismat,'input',1); 43 | % figure 44 | % plot(x,mf); 45 | % title('initial menbership functions') 46 | 47 | %使用函数anfis()进行神经模糊建摸 48 | numepochs=100; %训练次数40 49 | opt=anfisOptions("InitialFIS",fismat,"EpochNumber",numepochs,"ErrorGoal",0.005,"InitialStepSize",0.01,"StepSizeDecreaseRate",0.9,"ValidationData",chkdata); 50 | [fismat1,truerr,ss,fismat2,chkerr]=anfis(trndata,opt); 51 | %计算训练后神经模糊系统的输出与训练数据的均方根误差 52 | trnout=evalfis(trndata(:,1),fismat1); 53 | trnrmse=norm(trnout-trndata(:,2))/sqrt(length(trnout)); 54 | 55 | 56 | %使用函数tunefis()调整Mamdani模糊推理模型 57 | % 1学习规则 58 | options = tunefisOptions( 'Method' , 'particleswarm' , ... 59 | 'OptimizationType' , 'learning' , ... 60 | 'NumMaxRules' ,64); 61 | options.MethodOptions.MaxIterations = 50; 62 | options.UseParallel=true; 63 | % [in,out,rule] = getTunableSettings(fis); 64 | fisout1=tunefis(fisin,[],trndata(:,1),trndata(:,2),options); 65 | figure 66 | plotfis(fisout1) 67 | 68 | %2 调整所有参数 69 | [in,out,rule] = getTunableSettings(fisout1); 70 | options.OptimizationType = 'tuning'; 71 | options.Method = 'patternsearch'; 72 | options.MethodOptions.MaxIterations = 60; 73 | options.MethodOptions.UseCompletePoll = true; 74 | fisout = tunefis(fisout1,[in;out;rule],trndata(:,1),trndata(:,2),options); 75 | figure 76 | plotfis(fisout) 77 | 78 | 79 | %绘制训练过程中均方根误差的变化情况 80 | epoch=1:numepochs; 81 | figure 82 | plot(epoch,truerr,'o',epoch,chkerr,'x') 83 | hold on 84 | plot(epoch,[truerr,chkerr]); 85 | hold off 86 | title('误差曲线'); 87 | legend("训练集误差","验证集误差"); 88 | 89 | % %绘制训练过程中的步长的变化的情况 90 | % figure 91 | % plot(epoch,ss,'-',epoch,ss,'x'); 92 | % title('步长变化曲线') 93 | 94 | % %绘制训练后模糊推理系统的输入隶属度函数曲线 95 | % [x,mf]=plotmf(fismat1,'input',1); 96 | % figure 97 | % plot(x,mf) 98 | % title('训练后的输入隶属度函数'); 99 | 100 | % % %绘制训练后模糊推理系统的输入隶属度函数曲线 101 | % % % figure 102 | % plotmf(fismat1,'output',1); 103 | % title('训练后的输出隶属度函数'); 104 | 105 | %绘制神经模糊推理系统的输出曲线 106 | pre_y=evalfis(x1,fismat1); %系统辨识 107 | pre_y_1=evalfis(x1,fisout1); %系统辨识 108 | mse1=norm(y-d)/sqrt(length(d)); 109 | mse=norm(pre_y-d')/sqrt(length(d')); 110 | mse_mamfis=norm(pre_y_1-d')/sqrt(length(d')); 111 | 112 | figure 113 | plot(x1,d,'-r'); 114 | hold on; 115 | plot(x1,y,'-b'); 116 | hold on; 117 | plot(x1,pre_y_1,'--',"LineWidth",2) 118 | title('系统辨识曲线'); 119 | legend("理想系统",sprintf('辨识前的系统：误差%.03f',mse1),sprintf('模糊集合辨识后的系统：误差%.03f',mse_mamfis)); 120 | 121 | %总曲线 122 | figure 123 | plot(x1,d,'-r'); 124 | hold on; 125 | plot(x1,y,'-b'); 126 | hold on; 127 | plot(x1,pre_y,'--k',"LineWidth",2) 128 | hold on; 129 | plot(x1,pre_y_1,'--',"LineWidth",2) 130 | title('系统辨识曲线'); 131 | legend("理想系统",sprintf('辨识前的系统：误差%.03f',mse1),sprintf('T-S模型辨识后的系统：误差%.03f',mse),sprintf('模糊集合辨识后的系统：误差%.03f',mse_mamfis)); -------------------------------------------------------------------------------- /4/code/Untitled.m: -------------------------------------------------------------------------------- 1 | x=[0 100 3.263 Inf 0 2 | 1 200 3.263 5.688 0 3 | 2 300 3.263 4.717 1 4 | 3 400 2.875 5.089 0 5 | 4 500 2.875 4.749 1 6 | 5 600 2.875 4.855 2 7 | 6 700 2.875 4.905 3 8 | 7 800 2.875 4.803 4 9 | 8 900 2.875 4.784 5 10 | 9 1000 2.875 4.625 6 11 | 10 1100 2.875 4.358 7 12 | 11 1200 2.875 4.153 8 13 | 12 1300 2.82 4.125 0 14 | 13 1400 2.66 3.972 0 15 | 14 1500 2.376 3.887 0 16 | 15 1600 2.376 3.82 1 17 | 16 1700 2.376 3.71 2 18 | 17 1800 2.267 3.527 0 19 | 18 1900 2.267 3.507 1 20 | 19 2000 2.267 3.497 2 21 | 20 2100 2.267 3.418 3 22 | 21 2200 2.267 3.348 4 23 | 22 2300 2.255 3.233 0 24 | 23 2400 2.214 3.192 0 25 | 24 2500 2.132 3.255 0 26 | 25 2600 2.132 3.214 1 27 | 26 2700 2.132 3.23 2 28 | 27 2800 2.132 3.08 3 29 | 28 2900 2.132 3.13 4 30 | 29 3000 1.945 2.973 0 31 | 30 3100 1.917 2.951 0 32 | 31 3200 1.917 2.954 1 33 | 32 3300 1.917 2.901 2 34 | 33 3400 1.917 2.865 3 35 | 34 3500 1.917 2.831 4 36 | 35 3600 1.917 2.724 5 37 | 36 3700 1.917 2.703 6 38 | 37 3800 1.846 2.638 0 39 | 38 3900 1.846 2.541 0 40 | 39 4000 1.794 2.546 0 41 | 40 4100 1.759 2.508 0 42 | 41 4200 1.759 2.485 1 43 | 42 4300 1.694 2.407 0 44 | 43 4400 1.693 2.377 0 45 | 44 4500 1.693 2.338 1 46 | 45 4600 1.669 2.298 0 47 | 46 4700 1.669 2.225 1 48 | 47 4800 1.663 2.191 0 49 | 48 4900 1.647 2.134 0 50 | 49 5000 1.638 2.124 0 51 | 50 5100 1.626 2.041 0 52 | ]; 53 | x1=[0 1 1.62598 1 54 | 1 488 1.5148 2 55 | 2 959 1.45478 4 56 | 3 1395 1.42322 8 57 | 4 1574 1.38915 16 58 | 5 1656 1.34712 32 59 | 6 1688 1.34712 16 60 | 7 1769 1.32112 32 61 | 8 1802 1.32112 16 62 | 9 1882 1.30701 32 63 | 10 1915 1.30701 16 64 | 11 1995 1.30292 32 65 | 12 2028 1.30292 16 66 | 13 2108 1.2933 32 67 | 14 2141 1.2933 16 68 | 15 2221 1.28786 32 69 | 16 2254 1.28786 16 70 | 17 2334 1.28687 32 71 | 18 2367 1.28687 16 72 | 19 2447 1.28687 8 73 | 20 2625 1.23953 16 74 | 21 2705 1.23953 8 75 | 22 2883 1.20773 16 76 | 23 2964 1.20773 8 77 | 24 3142 1.18678 16 78 | 25 3223 1.18678 8 79 | 26 3401 1.16664 16 80 | 27 3482 1.16664 8 81 | 28 3660 1.15008 16 82 | 29 3741 1.15008 8 83 | 30 3918 1.14429 16 84 | 31 3999 1.14429 8 85 | 32 4176 1.14099 16 86 | 33 4257 1.14054 32 87 | 34 4290 1.14054 16 88 | 35 4370 1.14054 8 89 | 36 4546 1.13829 16 90 | 37 4625 1.13829 8 91 | 38 4801 1.13778 16 92 | 39 4880 1.11052 32 93 | 40 4913 1.11052 16 94 | 41 4992 1.11052 8 95 | 42 5168 1.11043 16 96 | 43 5247 1.11043 8 97 | 44 5422 1.11043 4 98 | 45 5858 1.10148 8 99 | 46 6034 1.10148 4 100 | 47 6470 1.09326 8 101 | 48 6646 1.09326 4 102 | 49 7082 1.08511 8 103 | 50 7258 1.08511 4 104 | 51 7694 1.07739 8 105 | 52 7870 1.07739 4 106 | 53 8306 1.06979 8 107 | 54 8482 1.06979 4 108 | 55 8918 1.06403 8 109 | 56 9095 1.06403 4 110 | 57 9530 1.05826 8 111 | 58 9707 1.05826 4 112 | 59 10141 1.05403 8 113 | 60 10318 1.05403 4 114 | 61 10752 1.05035 8 ]; 115 | plot(x1(:,3)); 116 | xlabel("epoch") 117 | ylabel("MSE") 118 | title("参数学习的误差曲线") -------------------------------------------------------------------------------- /4/code/fuzzy.m: -------------------------------------------------------------------------------- 1 | clear;clc;close all; 2 | % 观测点数 3 | N = 500; 4 | x = linspace(-2*pi, 2*pi,N ); 5 | % 信号 6 | y = 3 * sin(x)+2*cos(x.^2); 7 | % 噪声（方差1.25） 8 | v1 = 0.05*randn(N , 1).*y'; 9 | v2 = sqrt(0.5)*randn(N , 1); 10 | v=v1+v2;%噪声信号 11 | z=y'+v;%真实输出信号 12 | d=y';%期望输出信号 13 | P=x; 14 | T=z'; 15 | 16 | %% 画图 17 | plot(x,y); 18 | hold on; 19 | plot(x,T); 20 | axis([-2*pi 2*pi -6 6]); 21 | legend('y-x曲线','z-x曲线'); 22 | xlabel('横坐标 x'); 23 | ylabel('纵坐标 y'); 24 | 25 | figure; 26 | plot(x,v'); 27 | axis([-2*pi 2*pi -2 2]); 28 | legend('v-x曲线'); 29 | xlabel('横坐标 x'); 30 | ylabel('纵坐标 y'); 31 | 32 | % 数据集设置 33 | [a,b]=dividerand([P;T],0.8,0.2); %训练集+验证集 80%，测试集20% 34 | trainp=a(1,:); 35 | traint=a(2,:); 36 | testp=b(1,:); 37 | testt=b(2,:); -------------------------------------------------------------------------------- /4/code/untitled2.m: -------------------------------------------------------------------------------- 1 | a3=[-1.27155e-08 0.500095 8.88349e-15]; 2 | a4=[8.56018e-15 -8.88349e-15 0.500095]; 3 | a5=[-0.500095 -1.27155e-08 8.56018e-15]; 4 | a30=[7.10542e-14 1 1.77635e-14]; 5 | a31=[1.77635e-14 -1.77635e-14 1]; 6 | a32=[1 -7.10542e-14 -1.77635e-14]; 7 | 8 | b3=5.45921e-06; 9 | b4=-1.03236e-06; 10 | b5=-3.55471e-07; 11 | b30=1.88974e-07; 12 | b31=-2.6284e-08; 13 | b32=2.71588e-08; 14 | res=a3*b3+a4*b4+a5*b5; 15 | res2=a30*b30+a31*b31+a32*b32; 16 | res+res2 -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Changjing Liu 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Data Fusion 2 | > This are code repo for the Data Fusion course. 3 | 4 | ## Table of Contents 5 | * [Optimal Estimation](#optimal-estimation) 6 | * [Wiener Filter](#wiener-filter) 7 | * [Kalman Filter](#kalman-filter) 8 | * [Fuzzy Control](#fuzzy-control) 9 | * [Contact](#contact) 10 | 11 | 12 | 13 | ## Optimal Estimation 14 | ### Problem description 15 | Suppose a voltage is a random variable $X$ with normal distribution, the mean value is $5$, and the variance is $0.1$; The random variable x is measured $20$ times by two instruments, and the measurement error of the two instruments is assumed to be a normally distributed random variable with a mean value of $0$ and a variance of $0.1$ and $0.4$ respectively. Caculate the least square estimation (LSE), weighted least square estimation (WLS) and linear minimum variance estimation (LMMSE) of $X$, and calculate the mean square error of the corresponding estimation. Let the measurement equation be $Z=HZ+V$. 16 | 17 | ### Usage 18 | To handle the problem, run the following file: 19 | 20 | `1/code_1/main123.m` 21 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | ## Wiener Filter 36 | ### problem description 37 | Let $y (n) =x (n) +v (n)$, where $x(n)=10sin(\frac{\pi n}{128}+\frac{\pi}{3})$,$v(n)$ 38 | is white noise with variance of $1.25$. Design FIR and IIR Wiener filter to estimate the signal $x (n)$. 39 | ### Usage 40 | To handle the problem, run the following file: 41 | 42 | `1/code_1/main.m` 43 | 44 | 48 | 49 | ## Kalman Filter 50 | 51 | ### Basic Kalman Filter 52 | `1/code_3/kalman.m` 53 | ### Constant Gain Kalman Filter 54 | `1/code_3/kalman_constant_gain.m` 55 | ### Square root Kalman Filter 56 | `1/code_3/kalman_sqrt.m` 57 | ### Forgetting Factor Kalman Filter 58 | `1/code_3/kalman_forgetting_factor.m` 59 | ### Adaptive Kalman Filter 60 | `1/code_3/kalman_adaptive.m` 61 | ### Limited K Reduction Kalman Filter 62 | `1/code_3/kalman_restain_K.m` 63 | 64 | ### Extended Kalman Filter 65 | `2/code_0/EKF.m` 66 | ### Unscented Kalman Filter 67 | `2/code_0/UKF.m` 68 | ### Particle Filter 69 | `2/code_0/PF.m` 70 | 71 | ### Federated Kalman Filter 72 | `2/code_1/federated_filter.m` 73 | ### Decentralized Kalman filter 74 | `2/code_1/center_federated_filter.m` 75 | 76 | ## Fuzzy Control 77 | ### Basic method 78 | `4/code/TS_model.m` 79 | ### T-S method 80 | `4/code/TS_model.m` 81 | 82 | 83 | ## Contact 84 | changjingliu@sjtu.edu.cn 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | --------------------------------------------------------------------------------