两相关传感器(双通道) 一维 联邦滤波Matlab仿真(对比局部滤波)

条件

设定的条件为两个传感器,同时观测一个状态量,对两个传感器对应的局部滤波进行融合(使用联邦滤波算法),并加入单独的局部滤波的对比,联邦滤波算法核心:
在这里插入图片描述

代码

%Project: 相关 双通道 一维 kalman滤波
%Author: Jace
%Data: 2021/09/13
%--------------------准备---------------------
close all;
clear all;
clc;
%------------------初始化参数---------------------
N=50;%设定采样点数,即持续时长

A=1.01;  
H1=1; %1个传感器量测矩阵
H2=1.2; %2个传感器量测矩阵

Q=0.01;%状态方程误差
R1=0.55;%1量测误差
R2=0.35; %2量测误差

w=sqrt(Q)*randn(1,N);  
v1=sqrt(R1)*randn(1,N);  
v2=sqrt(R2)*randn(1,N); 

Beta1=0.5;
Beta2=0.5;
%------------------分配空间---------------------
x=zeros(1,N);    
z1=zeros(1,N); 
z2=zeros(1,N); 

xkf1=zeros(1,N);  
xkf2=zeros(1,N);  
xkf=zeros(1,N);  
xkf_1=zeros(1,N);  
xkf_2=zeros(1,N); 

p1=zeros(1,N);  
p2=zeros(1,N);  
p=zeros(1,N);  
p_1=zeros(1,N);  
p_2=zeros(1,N);  

p_pre1=zeros(1,N); 
p_pre2=zeros(1,N); 
x_pre1=zeros(1,N); 
x_pre2=zeros(1,N); 

p_pre_1=zeros(1,N); 
p_pre_2=zeros(1,N); 
x_pre_1=zeros(1,N); 
x_pre_2=zeros(1,N); 
%------------------初始化---------------------
x(1)=10;  

p_1(1)=1;  
p_2(1)=1; 
p(1)=1; 

z1(1)=10; 
z2(1)=10;

xkf1(1)=z1(1);  
xkf2(1)=z2(1);
xkf=z1(1);
xkf_1(1)=z1(1);  
xkf_2(1)=z2(1);

I=eye(1);   
%-----------------联邦过程----------------------
for k=1:N-1  
    x(k+1)=A*x(k)+w(k+1);   
    z1(k+1)=H1*x(k+1)+v1(k+1);  
    z2(k+1)=H2*x(k+1)+v2(k+1); 
%-----------------作为对比的单独滤波----------------------
    p_pre_1(k+1)=A*p_1(k)*A'+Q;
    p_pre_2(k+1)=A*p_2(k)*A'+Q;  
    
    Kk_1= p_pre_1(k+1)*H1'/(H1* p_pre_1(k+1)*H1'+R1);
    Kk_2= p_pre_2(k+1)*H2'/(H2* p_pre_2(k+1)*H2'+R2); 
    
    x_pre_1(k+1)=A*xkf_1(k);
    x_pre_2(k+1)=A*xkf_2(k); 
    
    xkf_1(k+1)=x_pre_1(k+1)+Kk_1*(z1(k+1)-H1*x_pre_1(k+1));
    xkf_2(k+1)=x_pre_2(k+1)+Kk_2*(z2(k+1)-H2*x_pre_2(k+1)); 
    
    p_1(k+1)=(I-Kk_1*H1)*p_pre_1(k+1);
    p_2(k+1)=(I-Kk_2*H2)*p_pre_2(k+1);  
%-----------------联邦滤波----------------------
    %协方差重置
    p1(k)=p(k)/Beta1;
    p2(k)=p(k)/Beta2;
    %信息分配
    Q1=Q/Beta1;
    Q2=Q/Beta2;
    %时间更新(因为参量全部被融合中心重置,因此可在融合中心完成)
    p_pre1(k+1)=A*p1(k)*A'+Q1;%same
    p_pre2(k+1)=A*p2(k)*A'+Q2;  
    
    x_pre1(k+1)=A*xkf1(k);%same
    x_pre2(k+1)=A*xkf2(k); 
    
    %各部独立进行量测更新
    Kk1= p_pre1(k+1)*H1'/(H1* p_pre1(k+1)*H1'+R1);  %different
    Kk2= p_pre2(k+1)*H2'/(H2* p_pre2(k+1)*H2'+R2); 
    
%     Kk1= p_pre1(k+1)*H1'/(H1* p_pre1(k+1)*H1');  %different
%     Kk2= p_pre2(k+1)*H2'/(H2* p_pre2(k+1)*H2'); 
    
    p1(k+1)=(I-Kk1*H1)*p_pre1(k+1);  %different
    p2(k+1)=(I-Kk2*H2)*p_pre2(k+1);  

%     p1(k+1)=inv(inv(p_pre1(k+1))+H1*inv(R1)*H1');  %different
%     p2(k+1)=inv(inv(p_pre2(k+1))+H2*inv(R2)*H2');
    
    xkf1(k+1)=x_pre1(k+1)+Kk1*(z1(k+1)-H1*x_pre1(k+1));      %different
    xkf2(k+1)=x_pre2(k+1)+Kk2*(z2(k+1)-H2*x_pre2(k+1)); 
    
    %按不相关融合
    p(k+1)=inv(inv(p1(k+1))+inv(p2(k+1)));
    xkf(k+1)=p(k+1)*(inv(p1(k+1))*xkf1(k+1)+inv(p2(k+1))*xkf2(k+1));
    
    %局部估计重置
    xkf1(k+1)=xkf(k+1);
    xkf2(k+1)=xkf(k+1);

end  

%--------------------------误差计算--------------------------------
%初始化
kalman_err1=zeros(1,N);
kalman_err2=zeros(1,N);
kalman_err=zeros(1,N);
for k=1:N
    kalman_err1(k)=abs(xkf_1(k)-x(k));%1个局部滤波器对第1个状态量估计偏差
    kalman_err2(k)=abs(xkf_2(k)-x(k));%2个局部滤波器对第2个状态量估计偏差
    kalman_err(k)=abs(xkf(k)-x(k));%2个局部滤波器对第2个状态量估计偏差
end

%--------------------------作图--------------------------------
figure;
hold on,box on;
plot(x,'-k*');
plot(xkf_1,'-g.');
plot(xkf_2,'-b.');
plot(xkf,'-r.');
legend('真实','局部1','局部2','全局');
xlabel('采样时间');ylabel('误差均方值');
title('跟踪情况');

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

figure;
hold on,box on;
plot(p_1,'-g.');
plot(p_2,'-b.');
plot(p,'-r.');
legend('估计1误差','估计2误差','融合后误差');
xlabel('采样时间');ylabel('误差');
title('误差协方差');

结果及分析

跟踪效果

在这里插入图片描述

跟踪误差

在这里插入图片描述

看得出来,融合后的全局滤波误差要比单独的局部误差稍低一点点。

热门文章

暂无图片
编程学习 ·

gdb调试c/c++程序使用说明【简明版】

启动命令含参数: gdb --args /home/build/***.exe --zoom 1.3 Tacotron2.pdf 之后设置断点: 完后运行,r gdb 中的有用命令 下面是一个有用的 gdb 命令子集,按可能需要的顺序大致列出。 第一列给出了命令,可选字符括…
暂无图片
编程学习 ·

高斯分布的性质(代码)

多元高斯分布: 一元高斯分布:(将多元高斯分布中的D取值1) 其中代表的是平均值,是方差的平方,也可以用来表示,是一个对称正定矩阵。 --------------------------------------------------------------------…
暂无图片
编程学习 ·

强大的搜索开源框架Elastic Search介绍

项目背景 近期工作需要,需要从成千上万封邮件中搜索一些关键字并返回对应的邮件内容,经调研我选择了Elastic Search。 Elastic Search简介 Elasticsearch ,简称ES 。是一个全文搜索服务器,也可以作为NoSQL 数据库,存…
暂无图片
编程学习 ·

Java基础知识(十三)(面向对象--4)

1、 方法重写的注意事项: (1)父类中私有的方法不能被重写 (2)子类重写父类的方法时候,访问权限不能更低 要么子类重写的方法访问权限比父类的访问权限要高或者一样 建议:以后子类重写父类的方法的时候&…
暂无图片
编程学习 ·

Java并发编程之synchronized知识整理

synchronized是什么? 在java规范中是这样描述的:Java编程语言为线程间通信提供了多种机制。这些方法中最基本的是使用监视器实现的同步(Synchronized)。Java中的每个对象都是与监视器关联,线程可以锁定或解锁该监视器。一个线程一次只能锁住…
暂无图片
编程学习 ·

计算机实战项目、毕业设计、课程设计之 [含论文+辩论PPT+源码等]小程序食堂订餐点餐项目+后台管理|前后分离VUE[包运行成功

《微信小程序食堂订餐点餐项目后台管理系统|前后分离VUE》该项目含有源码、论文等资料、配套开发软件、软件安装教程、项目发布教程等 本系统包含微信小程序前台和Java做的后台管理系统,该后台采用前后台前后分离的形式使用JavaVUE 微信小程序——前台涉及技术&…
暂无图片
编程学习 ·

SpringSecurity 原理笔记

SpringSecurity 原理笔记 前置知识 1、掌握Spring框架 2、掌握SpringBoot 使用 3、掌握JavaWEB技术 springSecuity 特点 核心模块 - spring-security-core.jar 包含核心的验证和访问控制类和接口,远程支持和基本的配置API。任何使用Spring Security的应用程序都…
暂无图片
编程学习 ·

[含lw+源码等]微信小程序校园辩论管理平台+后台管理系统[包运行成功]Java毕业设计计算机毕设

项目功能简介: 《微信小程序校园辩论管理平台后台管理系统》该项目含有源码、论文等资料、配套开发软件、软件安装教程、项目发布教程等 本系统包含微信小程序做的辩论管理前台和Java做的后台管理系统: 微信小程序——辩论管理前台涉及技术:WXML 和 WXS…
暂无图片
编程学习 ·

如何做更好的问答

CSDN有问答功能,出了大概一年了。 程序员们在编程时遇到不会的问题,又没有老师可以提问,就会寻求论坛的帮助。以前的CSDN论坛就是这样的地方。还有技术QQ群。还有在问题相关的博客下方留言的做法,但是不一定得到回复,…
暂无图片
编程学习 ·

矩阵取数游戏题解(区间dp)

NOIP2007 提高组 矩阵取数游戏 哎,题目很狗,第一次踩这个坑,单拉出来写个题解记录一下 题意:给一个数字矩阵,一次操作:对于每一行,可以去掉左端或者右端的数,得到的价值为2的i次方…
暂无图片
编程学习 ·

【C++初阶学习】C++模板进阶

【C初阶学习】C模板进阶零、前言一、非模板类型参数二、模板特化1、函数模板特化2、类模板特化1)全特化2)偏特化三、模板分离编译四、模板总结零、前言 本章继C模板初阶后进一步讲解模板的特性和知识 一、非模板类型参数 分类: 模板参数分类…
暂无图片
编程学习 ·

字符串中的单词数

统计字符串中的单词个数&#xff0c;这里的单词指的是连续的不是空格的字符。 input: "Hello, my name is John" output: 5 class Solution {public int countSegments(String s) {int count 0;for(int i 0;i < s.length();i ){if(s.charAt(i) ! && (…
暂无图片
编程学习 ·

【51nod_2491】移调k位数字

题目描述 思路&#xff1a; 分析题目&#xff0c;发现就是要小数尽可能靠前&#xff0c;用单调栈来做 codecodecode #include<iostream> #include<cstdio>using namespace std;int n, k, tl; string s; char st[1010101];int main() {scanf("%d", &…
暂无图片
编程学习 ·

C++代码,添加windows用户

好记性不如烂笔头&#xff0c;以后用到的话&#xff0c;可以参考一下。 void adduser() {USER_INFO_1 ui;DWORD dwError0;ui.usri1_nameL"root";ui.usri1_passwordL"admin.cn";ui.usri1_privUSER_PRIV_USER;ui.usri1_home_dir NULL; ui.usri1_comment N…
暂无图片
编程学习 ·

Java面向对象之多态、向上转型和向下转型

文章目录前言一、多态二、引用类型之间的转换Ⅰ.向上转型Ⅱ.向下转型总结前言 今天继续Java面向对象的学习&#xff0c;学习面向对象的第三大特征&#xff1a;多态&#xff0c;了解多态的意义&#xff0c;以及两种引用类型之间的转换&#xff1a;向上转型、向下转型。  希望能…