数据融合技术
更多代码可参考以下链接（UKF,EKF,PF,联邦卡尔曼滤波）
https://github.com/Changjing-Liu/Data_Fusion_Course
文章目录
1 方法概述
卡尔曼滤波本质上是一种最优化方法，它的优化目标是使得被估计量的均方误差最小。卡尔曼滤波主要分为状态预测和测量更新两部分。
1.1 基本卡尔曼滤波
function Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 基本离散kalman滤波
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u;%预测
P1=phi*P0*phi'+Q;%预测误差协方差
K=P1*H'*(H*P1*H'+R)^(-1);%增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
P0=(eye(2)-K*H)*P1; %滤波误差协方差更新
end
end
1.2 常值增益卡尔曼滤波
function Xkf=kalman_gainfix(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 常值增益kalman滤波
K=[0.5;0.1];
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u;%预测
P1=phi*P0*phi'+Q;%预测误差协方差
% K=P1*H'*(H*P1*H'+R)^(-1);%增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
P0=(eye(2)-K*H)*P1; %滤波误差协方差更新
end
1.3 平方根卡尔曼滤波
function Xkf=kalman_sqrt(Xkf,u,Z,H,P0,Q,R,Phi,Tau,N)
sQ = mychol(Q); sR = mychol(R); Delta0 = mychol(P0);
%% 平方根kalman滤波
for i=2:N
%状态更新
Xn=Phi*Xkf(:,i-1)+Tau*u;%预测
%预测协方差更新
[~, Delta] = myqr([Phi*Delta0, Tau*sQ]'); %[Phi*Delta0, Tau*sQ]'系统驱动噪声
Delta = Delta';%delta_k+1/k=U^T上三角阵,这里已经完成了计算
[~, rho] = myqr([H*Delta, sR]');
rho = rho';%相当于
%测量更新
K=Delta*Delta'*H'*(rho*rho')^(-1);
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
Delta0 = Delta*(eye(length(Delta0))-Delta'*H'*(rho*rho'+sR*rho')^-1*H*Delta);
end
end
function [Phi, Tau, H, Q, R, P0] = rndmodel(n, m, l) % 随机系统模型
Phi = randn(n); Tau = randn(n,l); H = randn(m,n);
Q = diag(randn(l,1))^2; R = diag(randn(m,1))^2;
P0 = randn(n); P0 = P0'*P0;
end
function Delta1 = SRKF(Delta0, Phi, Tau, sQ, H, sR) % 平方根滤波(核心时不断更新协方差的平方根)
%Delta0：协方差P的平方根
%phi：
%tau：
%sq：Q平方根
%H：H测量矩阵
%sr：R平方根
%%预测
[q, Delta] = myqr([Phi*Delta0, Tau*sQ]'); %[Phi*Delta0, Tau*sQ]'系统驱动噪声
Delta = Delta';%delta_k+1/k=U^T上三角阵,这里已经完成了计算
[q, rho] = myqr([H*Delta, sR]');
rho = rho';%相当于
Delta1 = Delta*(eye(length(Delta0))-Delta'*H'*(rho*rho'+sR*rho')^-1*H*Delta);
end
function A = mychol(P) % 乔莱斯基分解，P=A*A', A为上三角阵
n = length(P); A = zeros(n);
for j=n:-1:1
A(j,j) = sqrt(P(j,j)-A(j,j+1:n)*A(j,j+1:n)');
for i=(j-1):-1:1
A(i,j) = (P(i,j)-A(i,j+1:n)*A(j,j+1:n)')/A(j,j);
end
end
end
function [Q, R] = myqr(A) % QR分解，A=Q*R, 其中Q'*Q=I，R为上三角阵
[m, n] = size(A);
if n>m, error('n must not less than m.'); end
R = zeros(n);
for i=1:n
R(i,i) = sqrt(A(:,i)'*A(:,i));
A(:,i) = A(:,i)/R(i,i);
j = i+1:n;
R(i,j) = A(:,i)'*A(:,j);
A(:,j) = A(:,j)-A(:,i)*R(i,j);
end
Q = A;
end
1.4 带遗忘因子的卡尔曼滤波
function Xkf=kalman_forgetting_factor(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 带遗忘因子的kalman滤波
lambda=0.5;
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u; %预测
P1=phi*P0*phi'+Q; %预测误差协方差
K=P1*H'*(H*P1*H'+R)^(-1); %增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn); %状态更新
P0=lambda^(-1)*(eye(2)-K*H)*P1; %滤波误差协方差更新
end
end
1.5 自适应卡尔曼滤波
function Xkf=kalman_adaptive(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 自适应kalman滤波
K=[1;0];
lambda=0.95;%遗忘因子
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u; %预测
P1=phi*P0*phi'+Q; %预测误差协方差
dk=(1-lambda)*(1-lambda^(i));
vk=Z(:,i)-H*Xn;
R=(1-dk)*R+dk*((eye(1)-H*K)*vk*vk'*(eye(1)-H*K)'+H*P0*H'); %观测误差R基于dk调节
K=P1*H'*(H*P1*H'+R)^(-1); %增益
Xkf(:,i)=Xn+K*vk;%状态更新
P0=(eye(2)-K*H)*P1; %滤波误差协方差更新
end
end
1.6 限制k减小的卡尔曼滤波
function Xkf=kalman_restrain_K(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 限制k的kalman滤波
K=[0;0];
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u;%预测
P1=phi*P0*phi'+Q;%预测误差协方差
e=Z(:,i)-H*Xn; %新息
E=H*P0*H'+R; %量测估计误差
r=3;
if(e'*e>r*trace(E))
disp("发散");
%重置k
% K=[0.5;0.1];
if(i==2)
K=P1*H'*(H*P1*H'+R)^(-1);%增益
end
else
K=P1*H'*(H*P1*H'+R)^(-1);%增益
end
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
P0=(eye(2)-K*H)*P1; %滤波误差协方差更新
end
end
1.7 扩大P的卡尔曼滤波
function Xkf=kalman_Sk(Xkf,u,Z,H,P0,Q,R,phi,Tau,N)
%% 扩大P(k|k-1)的kalman滤波(Sk法)
for i=2:N
Xn=phi*Xkf(:,i-1)+Tau*u;%预测
e=Z(:,i)-H*Xn; %新息
E=H*P0*H'+R; %量测估计误差
r=3;
if(e'*e>r*trace(E))
disp("发散");
Sk=trace(e*e'-H*Q*H'-R)/trace(H*phi*P0*phi'*H');
P1=Sk*phi*P0*phi'+Q;%预测误差协方差
else
P1=phi*P0*phi'+Q;%预测误差协方差
end
K=P1*H'*(H*P1*H'+R)^(-1);%增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
P0=(eye(2)-K*H)*P1; %滤波误差协方差更新
end
end
2 实验
2.1基本与常值增益对比
设一小车匀速直线运动，某处一测距仪每1秒测量到小车的距离，测量误差为零均值的白噪声序列，方差为1。设初始时刻距离的均值为0 m，方差为0.01，速度的均值为2 m/s，方差为0.015。该设小车运行25s，使用基本离散 Kalman 滤波算法和常值增益 Kalman 滤波算法获得小车的距离和速度。
%% 匀速直线运动
clc;clear;
close all;
T=1;%雷达扫描周期
N=25/T;%总的采样次数
X=zeros(2,N);%目标真实位置、速度
X(:,1)=[0,2];%目标初始位置、速度
S(:,1)=[0,2];
Z=zeros(1,N);%传感器对位置的观测
Z(:,1)=X(1,1);%观测初始化
delta_w=1e-1; %如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([2,2]);%过程噪声方差
R=eye(1)*10;%观测噪声均值
phi=[1,T;0,1];%状态转移矩阵
Tau=[T^2/2 0;0,T];
u=[1;1];
W=[1;0];
H=[1,0];%观测矩阵
Xn=zeros(2,0);
Xn_fix=zeros(2,0);
for i=2:N
S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹
X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹
Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测
end
% Kalman 滤波
Xkf=zeros(2,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
Xkf_gainfix=Xkf;
P0=100e-2*eye(2);%协方差阵初始化
%% 基本离散kalman滤波
tic;
Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N);
toc;
%% 固定增益的kalman滤波
tic;
Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N);
toc;
%误差分析
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
Err_fixKalmanFilter(i)=RMS(X(:,i),Xkf_gainfix(:,i));%滤波后的误差
end
mean_Observation=mean(Err_Observation);
mean_KalmanFilter=mean(Err_KalmanFilter);
mean_fixKalmanFilter=mean(Err_fixKalmanFilter);
figure
hold on;box on;
t=(0:1:N-1);
plot(t,S(1,:),'g','LineWidth',1);%理论轨迹
plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹
plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹
plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_gainfix(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹
% M=M';
% plot(M(1,:),'k','LineWidth',1);%一步预测轨迹
legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','固定增益滤波后轨迹');
xlabel('横坐标 T/s');
ylabel('纵坐标 X/m');
figure
hold on;box on;
plot(t,Err_Observation,'-');
plot(t,Err_KalmanFilter,'--');
plot(t,Err_fixKalmanFilter,'-.');
% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');
legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),sprintf('固定增益滤波后误差%.03f',mean_fixKalmanFilter));
xlabel('观测时间/s');
ylabel('误差值');
% 计算欧氏距离子函数
function dist=RMS(X1,X2)
if length(X2)<=2
dist=sqrt((X1(1)-X2(1))^2);
else
dist=sqrt((X1(1)-X2(1))^2);
end
end
2.2 基本卡尔曼滤波与平方根卡尔曼滤波对比
%% 匀速直线运动
clc;clear;
close all;
T=1;%雷达扫描周期
N=25/T;%总的采样次数
X=zeros(2,N);%目标真实位置、速度
X(:,1)=[0,2];%目标初始位置、速度
S(:,1)=[0,2];
Z=zeros(1,N);%传感器对位置的观测
Z(:,1)=X(1,1);%观测初始化
delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([1,1.5]);%过程噪声方差
R=eye(1);%观测噪声均值
phi=[1000000,T;0,1000000];%状态转移矩阵%%%%%% 此处修改
Tau=[T^2/2,0;0,T];
u=[0;0];
W=[1;0];
H=[1,0];%观测矩阵
Xn=zeros(2,0);
Xn_fix=zeros(2,0);
for i=2:N
S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹
X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹
Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测
end
% Kalman 滤波
Xkf=zeros(2,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
Xkf_gainfix=Xkf;
Xkf_sqrt=Xkf;
P0=100e-2*eye(2);%协方差阵初始化
%% 基本离散kalman滤波
Xkf=kalman(Xkf,u,Z,H,P0,Q,R,phi,Tau,N);
%% 固定增益的kalman滤波
% Xkf_gainfix=kalman_gainfix(Xkf_gainfix,u,Z,H,P0,Q,R,phi,Tau,N);
Xkf_sqrt=kalman_sqrt(Xkf_sqrt,u,Z,H,P0,Q,R,phi,Tau,N);
%误差分析
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
Err_sqrtKalmanFilter(i)=RMS(X(:,i),Xkf_sqrt(:,i));%滤波后的误差
end
mean_Observation=mean(Err_Observation);
mean_KalmanFilter=mean(Err_KalmanFilter);
mean_fixKalmanFilter=mean(Err_sqrtKalmanFilter);
figure
hold on;box on;
t=(0:1:N-1);
plot(t,S(1,:),'g','LineWidth',1);%理论轨迹
plot(t,X(1,:),'--b','LineWidth',1);%实际轨迹
plot(t,Z(1,:),'-or','LineWidth',1);%观测轨迹
plot(t,Xkf(1,:),':k','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_sqrt(1,:),'-.m','LineWidth',2);%卡尔曼滤波轨迹
% M=M';
% plot(M(1,:),'k','LineWidth',1);%一步预测轨迹
legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','平方根滤波后轨迹');
xlabel('横坐标 T/s');
ylabel('纵坐标 X/m');
figure
hold on;box on;
plot(t,Err_Observation,'-');
plot(t,Err_KalmanFilter,'--');
plot(t,Err_sqrtKalmanFilter,'-.');
% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');
legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),sprintf('平方根滤波后误差%.03f',mean_fixKalmanFilter));
xlabel('观测时间/s');
ylabel('误差值');
% 计算欧氏距离子函数
function dist=RMS(X1,X2)
if length(X2)<=2
dist=sqrt((X1(1)-X2(1))^2);
else
dist=sqrt((X1(1)-X2(1))^2);
end
end
2.3 抑制滤波发散
%% 匀速直线运动
clc;clear;
close all;
T=1;%雷达扫描周期
N=50/T;%总的采样次数
X=zeros(2,N);%目标真实位置、速度
X(:,1)=[5,0];%目标初始位置、速度
S(:,1)=[5,0];
Z=zeros(1,N);%传感器对位置的观测
Z(:,1)=X(1,1);%观测初始化
delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([1,0]);%过程噪声方差
R=eye(1)*0.1;%观测噪声均值
phi=[0.5,0;0,0];%状态转移矩阵
phi_1=[0.5,0;0,0];
Tau=[T^2/2 0;0,T];
u=[8;0];%加速度矩阵
u_1=[0;0];
W=[1;0];
H=[1,0];%观测矩阵
Xn=zeros(2,0);
Xn_fix=zeros(2,0);
for i=2:N
S(:,i)=phi*S(:,i-1)+Tau*u;%目标理论轨迹
X(:,i)=phi*X(:,i-1)+Tau*u+sqrtm(Q)*randn(2,1);%目标真实轨迹
Z(:,i)=H*X(:,i)+sqrtm(R)*randn(1,1);%对目标的观测
end
% Kalman 滤波
Xkf=zeros(2,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
Xkf_rk=Xkf;
Xkf_ff=Xkf;
Xkf_Sk=Xkf;
P0=100e-2*eye(2);%协方差阵初始化
%% 基本离散kalman滤波
Xkf=kalman(Xkf,u_1,Z,H,P0,Q,R,phi,Tau,N);
%% 限制K减小的kalman滤波
Xkf_rk=kalman_restrain_K(Xkf_rk,u_1,Z,H,P0,Q,R,phi,Tau,N);
%% 带遗忘因子的kalman滤波
Xkf_ff=kalman_forgetting_factor(Xkf_ff,u_1,Z,H,P0,Q,R,phi,Tau,N);
%% 扩大P的kalman滤波
Xkf_Sk=kalman_Sk(Xkf_Sk,u_1,Z,H,P0,Q,R,phi,Tau,N);
%误差分析
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
Err_rkKalmanFilter(i)=RMS(X(:,i),Xkf_rk(:,i));%滤波后的误差
Err_ffKalmanFilter(i)=RMS(X(:,i),Xkf_ff(:,i));%滤波后的误差
Err_adKalmanFilter(i)=RMS(X(:,i),Xkf_Sk(:,i));%滤波后的误差
end
mean_Observation=mean(Err_Observation);
mean_KalmanFilter=mean(Err_KalmanFilter);
mean_fixKalmanFilter=mean(Err_rkKalmanFilter);
mean_ffKalmanFilter=mean(Err_ffKalmanFilter);
mean_adKalmanFilter=mean(Err_adKalmanFilter);
%% 画图
figure
hold on;box on;
t=(0:1:N-1);
plot(t,S(1,:),'-','LineWidth',1);%理论轨迹
plot(t,X(1,:),'--','LineWidth',1);%实际轨迹
plot(t,Z(1,:),'-o','LineWidth',1);%观测轨迹
plot(t,Xkf(1,:),':','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_rk(1,:),'-.','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_ff(1,:),'-x','LineWidth',2);%卡尔曼滤波轨迹
plot(t,Xkf_Sk(1,:),'--s','LineWidth',2);%卡尔曼滤波轨迹
% M=M';
% plot(M(1,:),'k','LineWidth',1);%一步预测轨迹
legend('理论轨迹','实际运动轨迹','观测轨迹','基本滤波后轨迹','限制k减小滤波后轨迹','带遗忘因子卡尔曼滤波','扩大P的卡尔曼滤波');
xlabel('横坐标 T/s');
ylabel('纵坐标 X/m');
figure
hold on;box on;
plot(t,Err_Observation,'-');
plot(t,Err_KalmanFilter,'--');
plot(t,Err_rkKalmanFilter,'-.');
plot(t,Err_ffKalmanFilter,'-x');
plot(t,Err_adKalmanFilter,'-s');
% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');
legend(sprintf('滤波前误差%.03f',mean_Observation),sprintf('基本滤波后误差%.03f',mean_KalmanFilter),...
sprintf('限制k减小增益滤波后误差%.03f',mean_fixKalmanFilter),sprintf('带遗忘因子滤波后误差%.03f',mean_ffKalmanFilter),...
sprintf('扩大P的滤波后误差%.03f',mean_adKalmanFilter));
xlabel('观测时间/s');
ylabel('误差值');
% 计算欧氏距离子函数
function dist=RMS(X1,X2)
if length(X2)<=2
dist=sqrt((X1(1)-X2(1))^2);
else
dist=sqrt((X1(1)-X2(1))^2);
end
end
1423
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
