联邦滤波算法封装Matlab函数

Jace Lee

已于 2022-04-22 10:53:58 修改

阅读量1.9k

点赞数 10

分类专栏： Matlab 算法安全估计文章标签：算法 matlab 卡尔曼滤波算法

于 2021-11-02 15:37:33 首次发布

本文链接：https://blog.csdn.net/yujian2563/article/details/121095046

版权

算法同时被 3 个专栏收录

19 篇文章 11 订阅

订阅专栏

Matlab

11 篇文章 3 订阅

订阅专栏

安全估计

9 篇文章 0 订阅

订阅专栏

文章目录

背景
函数代码
调用方法
调用测试函数

背景

本人最近需要写多个仿真，需要用到联邦滤波Federal Filter算法效果对比，于是干脆将联邦滤波算法封装为函数，后续使用直接进行调用即可。

函数代码

注意，本函数依赖于LKF函数定义，参考博文

%Project: 全维度联邦滤波函数
%Author: Jace
%Data: 2021/11/2
%====================参数说明====================
%由于外部状态量的维度未知，因此不能一个一个地传入单一维度的参数，
%需要将所有维度的同一个参数【按行】压缩为一个增广参数矩阵，因此对需要做
%增广的参数做出说明：

%外部增广 Beta=cat(2,Beta1,Beta2,Beta3,...);
%外部增广 R=cat(2,R1,R2,R3,...);
%外部增广 H=cat(2,H1,H2,H3,...);
%外部增广 Z=cat(2,z1,z2,z3,...);

%====================函数体====================
function [xkfo,P]=FederalFilter(Dim_n,Dim_m,A,H,Gamma,Q,R,Beta,Z,xkfopre,Ppre)
    %====================分配空间========================
    Qin=zeros(1,Dim_n);
    K=zeros(Dim_n,Dim_m,Dim_n);
    Pprein=zeros(Dim_n,Dim_n,Dim_n);
    P_pre=zeros(Dim_n,Dim_n,Dim_n);
    P=zeros(Dim_n,Dim_n,Dim_n);
    xkf=zeros(Dim_n,Dim_n);
    %========================初始化========================
    %--------运算参数初始化--------
    Sigma=0;
    Alpha=0;
    %--------特定矩阵初始化--------
    In=eye(Dim_n); 
    %========================依照状态维度循环遍历========================
    for k=1:Dim_n
        %========================联邦滤波部分========================
        %--------重置--------
        %协方差重置
        Pprein(:,:,k)=Ppre/Beta(k);
        %信息分配 
        Qin(k)=Q/Beta(k);
        
        %--------Kalman--------
        %时间更新（因为参量全部被融合中心重置，因此可在融合中心完成）
        P_pre(:,:,k)=A*Pprein(:,:,k)*A'+Gamma*Qin(k)*Gamma'; 

        %各部独立进行量测更新
        K(:,:,k)= P_pre(:,:,k)*H(:,(k-1)*Dim_n+1:k*Dim_n)'/(H(:,(k-1)*Dim_n+1:k*Dim_n)*P_pre(:,:,k)*H(:,(k-1)*Dim_n+1:k*Dim_n)'+R(:,(k-1)*Dim_m+1:k*Dim_m)); 
        P(:,:,k)=(In-K(:,:,k)*H(:,(k-1)*Dim_n+1:k*Dim_n))*P_pre(:,:,k); 
        xkf(:,k)=A*xkfopre+K(:,:,k)*(Z(:,k)-H(:,(k-1)*Dim_n+1:k*Dim_n)*A*xkfopre);
    end
    %--------联邦滤波中按不相关融合--------
    %按照状态维度加和
    for k=1:Dim_n
        Sigma=Sigma+inv(P(:,:,k));
        Alpha=Alpha+inv(P(:,:,k))*xkf(:,k);
    end
    P=inv(Sigma);
    xkfo=P*Alpha;
end

调用方法

[xkfo,P]=FederalFilter(Dim_n,Dim_m,A,H,Gamma,Q,R,Beta,Z,xkfopre,Ppre)

对应

[当前时刻联邦估计值xkfo,当前时刻联邦估计误差协方差P]=FederalFilter(状态维度Dim_n,量测维度Dim_m,状态矩阵A,量测矩阵H,状态噪声矩阵Gamma,状态噪声协方差Q,量测噪声协方差R,信息分配系数Beta,本时刻量测值Z,上一时刻联邦估计值xkfopre,上一时刻联邦误差协方差Ppre)

注意：需要提前对相关参数Beta、R、H、Z做如下增广：

%外部增广 Beta=cat(2,Beta1,Beta2,Beta3,...);
%外部增广 R=cat(2,R1,R2,R3,...);
%外部增广 H=cat(2,H1,H2,H3,...);
%外部增广 Z=cat(2,z1,z2,z3,...);

调用测试函数

%Project: 三通道三维联邦滤波
%Author: Jace
%Data: 2021/10/31
%====================准备====================
close all;
clear all;
clc;
%====================初始化参数====================
%--------全局参数--------
N=1000;

%--------设定维度--------
Dim_n=3;
Dim_m=1;

%--------系统模型参数--------
T=0.1;
A=[1,T,T^2/2;
    0,1,T;
    0,0,1];

H1=[1,0,0];%量测矩阵
H2=[1,2,0];
H3=[1,1,1];

Gamma=[0.2;0.1;1];

%--------噪声相关参数--------
Q=1;%设定系统噪声
R1=eye(Dim_m);%设定观测噪声
R2=1.1*eye(Dim_m);%设定观测噪声
R3=0.9*eye(Dim_m);%设定观测噪声
w=sqrt(Q)*randn(Dim_n,N);
v1=sqrt(R1)*randn(Dim_m,N);
v2=sqrt(R2)*randn(Dim_m,N);
v3=sqrt(R3)*randn(Dim_m,N);

%--------信息分配相关参数--------
Beta1=1/3;
Beta2=1/3;
Beta3=1/3;

%====================分配空间========================
%--------系统参数--------
x=zeros(Dim_n,N);    
z1=zeros(Dim_m,N); 
z2=zeros(Dim_m,N); 
z3=zeros(Dim_m,N); 

%--------Kalman过程参数--------
xkf1=zeros(Dim_n,N);  
xkf2=zeros(Dim_n,N);  
xkf3=zeros(Dim_n,N);

xkfo=zeros(Dim_n,N); 

xkfcomp1=zeros(Dim_n,N);  
xkfcomp2=zeros(Dim_n,N); 
xkfcomp3=zeros(Dim_n,N); 

p1=zeros(Dim_n,Dim_n,N);  
p2=zeros(Dim_n,Dim_n,N);  
p3=zeros(Dim_n,Dim_n,N);  
p=zeros(Dim_n,Dim_n,N);  

pcomp1=zeros(Dim_n,Dim_n,N);  
pcomp2=zeros(Dim_n,Dim_n,N);  
pcomp3=zeros(Dim_n,Dim_n,N);  
%====================初始化====================
%--------系统参数初始化--------
z1(:,1)=v1(:,1); 
z2(:,1)=v2(:,1);
z3(:,1)=v3(:,1);

%--------估计参数初始化--------
xkf1(:,1)=x(:,1);  
xkf2(:,1)=x(:,1);
xkf3(:,1)=x(:,1);
xkfo(:,1)=x(:,1);

xkfcomp1(:,1)=x(:,1);  
xkfcomp2(:,1)=x(:,1);
xkfcomp3(:,1)=x(:,1);

pcomp1(:,:,1)=0.1;  
pcomp2(:,:,1)=0.1; 
pcomp3(:,:,1)=0.1; 

p(:,:,1)=0.1*eye(Dim_n); 

%--------特定矩阵初始化--------
In=eye(Dim_n);  

%====================迭代过程====================
for k=2:N 
    x(:,k)=A*x(:,k-1)+w(k);   
    z1(:,k)=H1*x(:,k)+v1(:,k);  
    z2(:,k)=H2*x(:,k)+v2(:,k); 
    z3(:,k)=H3*x(:,k)+v3(:,k); 
    %====================作为对比的单独滤波====================
    [pcomp1(:,:,k),xkfcomp1(:,k)]=Lkf(Dim_n,A,H1,Gamma,Q,R1,pcomp1(:,:,k-1),xkfcomp1(:,k-1),z1(:,k));
    [pcomp2(:,:,k),xkfcomp2(:,k)]=Lkf(Dim_n,A,H2,Gamma,Q,R2,pcomp2(:,:,k-1),xkfcomp2(:,k-1),z2(:,k));
    [pcomp3(:,:,k),xkfcomp3(:,k)]=Lkf(Dim_n,A,H3,Gamma,Q,R3,pcomp3(:,:,k-1),xkfcomp3(:,k-1),z3(:,k));
    %====================联邦滤波====================
    %调用全维度联邦滤波函数，预处理参数矩阵做增广
    Z=cat(2,z1(:,k),z2(:,k),z3(:,k));
    Beta=cat(2,Beta1,Beta2,Beta3);
    R=cat(2,R1,R2,R3);
    H=cat(2,H1,H2,H3);
    %调用全维度联邦滤波函数
    [xkfo(:,k),p(:,:,k)]=FederalFilter(Dim_n,Dim_m,A,H,Gamma,Q,R,Beta,Z,xkfo(:,k-1),p(:,:,k-1));
end  
    
%====================实时误差计算====================
kalman_err1=zeros(1,N);
kalman_err2=zeros(1,N);
kalman_err3=zeros(1,N);
kalman_err=zeros(1,N);
for k=1:N
    kalman_err1(k)=abs(xkfcomp1(1,k)-x(1,k));%第1个局部滤波器对第1个状态量估计偏差
    kalman_err2(k)=abs(xkfcomp2(1,k)-x(1,k));%第2个局部滤波器对第1个状态量估计偏差
    kalman_err3(k)=abs(xkfcomp3(1,k)-x(1,k));%第3个局部滤波器对第1个状态量估计偏差
    kalman_err(k)=abs(xkfo(1,k)-x(1,k));%全局滤波器对第1个状态量估计偏差
end

%====================MSE计算====================
Dim=3;
Step=100;
MSE_DFFWM=MSE(Dim,Step,N,xkfo,x);
MSE_LKF1=MSE(Dim,Step,N,xkfcomp1,x);
MSE_LKF2=MSE(Dim,Step,N,xkfcomp2,x);
MSE_LKF3=MSE(Dim,Step,N,xkfcomp3,x);
%====================绘图====================
figure;
hold on,box on;
plot(x(1,:),'-k.');
plot(xkfcomp1(1,:),'-r.');
plot(xkfcomp2(1,:),'-g.');
plot(xkfcomp3(1,:),'-b.');
plot(xkfo(1,:),'-c.');
legend('真实','局部1','局部2','局部3','全局');
xlabel('采样时间');ylabel('误差均方值');
title('跟踪情况');

figure;
hold on,box on;
plot(kalman_err1,'-r.');
plot(kalman_err2,'-g.');
plot(kalman_err3,'-b.');
plot(kalman_err,'-c.');
legend('估计1误差','估计2误差','估计3误差','融合后误差');
xlabel('采样时间');ylabel('误差');
title('滤波器误差');

figure;
hold on,box on;
plot(MSE_DFFWM(1,:),'-r.');
plot(MSE_LKF1(1,:),'-g.');
plot(MSE_LKF2(1,:),'-b.');
plot(MSE_LKF3(1,:),'-c.');
legend('MSE4DFFWM','MSE4LKF1','MSE4LKF2','MSE4LKF3');
xlabel('采样时间');ylabel('值');
title('第一状态值MSE');

figure;
hold on,box on;
plot(MSE_DFFWM(2,:),'-r.');
plot(MSE_LKF1(2,:),'-g.');
plot(MSE_LKF2(2,:),'-b.');
plot(MSE_LKF3(2,:),'-c.');
legend('MSE4DFFWM','MSE4LKF1','MSE4LKF2','MSE4LKF3');
xlabel('采样时间');ylabel('值');
title('第二状态值MSE');

figure;
hold on,box on;
plot(MSE_DFFWM(3,:),'-r.');
plot(MSE_LKF1(3,:),'-g.');
plot(MSE_LKF2(3,:),'-b.');
plot(MSE_LKF3(3,:),'-c.');
legend('MSE4DFFWM','MSE4LKF1','MSE4LKF2','MSE4LKF3');
xlabel('采样时间');ylabel('值');
title('第三状态值MSE');