两不相关传感器（双通道） 二维 kalman滤波Matlab仿真

条件

设定的条件为两个传感器，每个传感器独立观测一个状态量，但两个状态量之间存在耦合（A矩阵非对角线上不为0）的情况下，对两个传感器对应的局部滤波进行融合（使用的是最简单的算法）：

两个传感器下就是：

代码

%Project: 不相关&双通道&二维&滚动窗检测器
%Author: Jace
%Data: 2021/09/15
%--------------------准备---------------------
close all;
clear all;
clc;
%------------------初始化参数---------------------
N=50;%设定采样点数，即持续时长

%噪声相关参数
Q=[0.01,0.0;0.0,0.05];%设定系统噪声
R1=0.28;%设定局部观测1噪声
R2=0.25;%设定局部观测2噪声
w=sqrt(Q)*randn(2,N);
v1=sqrt(R1)*randn(1,N);
v2=sqrt(R2)*randn(1,N);

%系统模型参数
A=[0.8,0.1;0.5,0.7];%状态转移矩阵
H1=[1,0];%局部量测1量测矩阵
H2=[0,1];%局部量测2量测矩阵

%----------------初始化分配空间-------------------------
%真实状态值初始化
x=zeros(2,N);%物体真实状态值（分配空间），2*N维矩阵
x(:,1)=[10;20];%物体初始真实状态值

%误差协方差初始化
p1=[1,0;0,1];%局部滤波1误差协方差初始值
p2=[1,0;0,1];%局部滤波2误差协方差初始值
p=[1,0;0,1];%全局滤波误差协方差初始值

%两个传感器量测值初始化
z1=zeros(1,N);%量测值1
z1(1)=H1*x(:,1);%观测1真实值初始值,第一列的列向量取出第一行，成为标量
z2=zeros(1,N);%量测值2
z2(1)=H2*x(:,1);%观测2真实值初始值,第一列的列向量取出第二行，成为标量

%各滤波器估计值初始化
xkf1=zeros(2,N);%局部估计状态1
xkf1