前面的博客中讲得都是针对一种传感器的测量值有状态空间模型对系统状态值进行估计,本次博客讨论多传感器下对系统状态值估计的方法,即多传感器的数据融合。多传感器的状态融合又分成两种:1.集中式kalman滤波;2.分布式卡尔曼滤波。集中式kalman滤波是通过将多个观测方程集中为一个观测方程,然后进行标准kalman滤波,该方法可以得到全局的最优估计,但是缺点也是很明显,多个观测方程集中为一个观测方程,必然导致系统观测维度的增加,维度增加大大增加了标准kalman滤波的计算量,同时将多个系统集中到一个系统中,会导致该系统的容错性差,容错性对于系统的重要性可能比导航精度更加重要,所以集中式kalman滤波适用范围较小;分布式kalman滤波是将多个传感器各自组成多个kalman滤波子系统,进行标准kalman滤波,最后根据最小二乘法对状态估计进行融合,得到融合结果。由于集中式kalman滤波适用范围较小,所以本博客主要介绍分布式kalman滤波。
在线性最小方差意义下,加权融合存在三种加权方式:1.按矩阵加权;2.按标量加权;3.按对角线加权。这三种加权方式的优缺点主要从计算量和误差的角度进行分析。计算量:矩阵加权>按对角线加权>按标量加权;误差:按标量加权>按对角线加权>矩阵加权。这里我对上述三种加权方式给出理性的一些认识,矩阵加权说明权重是一个矩阵,既然是矩阵说明在计算这个权重矩阵时考虑到因素就多,对于融合后状态值也是多个状态值组合构成的,所以该方法计算出来的估计值估计误差肯定小;按照标量加权权重是一个标量值,考虑到的因素肯定少很多,所以估计误差会差很多;按对角线加权实际上也是标量加权,与标量加权不同的是标量加权对于所有的状态值估计权重是一样的,但是按照对角线加权对于多个状态值,每个状态值都有相应的权重,这样做肯定要比标量加权的效果要好,所以估计误差小于标量加权。下面对上述三种加权方式进行推导。
假设有随机向量x,基于L个传感器观测已知它的无偏估计为:,且已知上述估计误差的方差阵和协方差矩阵,寻求x的矩阵加权融合估计,其表达式如下:
(1)
其中:
(2)
下面是求权重矩阵Ai的过程:
(3)
(4)
则有:
(5)
这里不妨令:
(6)
(7)
则有:
(8)
要求J的最小下,矩阵A的值,但是矩阵A存在约束条件式2,采用拉格朗日乘数法,建立辅助函数可得:
(9)
则有对矩阵A求微分,得:
(10)
对参数微分可得:
(11)
对式10和式11进行整理可得:
对于式10,存在:
(12)
其中:
(13)
则有:
(14)
对于式11,有:
(15)
联立式14和式15,则有:
(16)
解式16矩阵可得:
(17)
所以有:
(18)
下面来计算该权重下,状态估计的协方差矩阵,根据比较协方差矩阵的迹来比较误差的大小。
(19)
根据式19有:
(20)
将式18带入式20,可得:
(21)
则有:
(22)
这说明融合后的状态估计精度高宇每个局部估计精度。
假设有随机向量x,基于L个传感器观测已知它的无偏估计为:,且已知上述估计误差的方差阵和协方差矩阵,寻求x的矩阵加权融合估计,其表达式如下:
(22)
其中有:
(23)
其中ai为标量,下面我们来求标量表达式。
线性最小方差意义下的极小化性能指标为:
(24)
其中有:
(25)
带入式21,可得:
(26)
之后拉格朗日乘数法求最优解可得:
(27)
对向量a进行微分可得:
(28)
对参数微分可得:
(29)
式25和式26联立可得:
(30)
由此可得
(31)
同理这里求融合估计协方差:
则有:
(32)
可以证明:
(33)
这说明融合后的状态估计精度高宇每个局部估计精度。
假设有随机向量x,基于L个传感器观测已知它的无偏估计为:,且已知上述估计误差的方差阵和协方差矩阵,寻求x的矩阵加权融合估计,其表达式如下:
(34)
则有:
(35)
线性最小方差意义下的极小化性能指标为:
(36)
其中:
(37)
(38)
其中为协方差矩阵P11的第i个对角线上的元素。
之后拉格朗日乘数法求最优解可得:
(39)
对参数ai微分可得:
(40)
对参数微分可得:
(41)
联立式40和式41可得:
(42)
由此可得:
(43)
下面来求协方差矩阵:
(44)
则有:
(45)
可以证明:
(46)
这说明融合后的状态估计精度高宇每个局部估计精度。
仿真测试:
考虑带有色观测噪声的3传感器时变系统
其中参数有:
其中
,b(t)为bernoulli白噪声,仅取0或者1,取值概率为:,,g(t)是零均值,方差为0.03的高斯白噪声,分别为方差为0.001,0.002,0.003的高斯白噪声,仿真三种传感器融合结果。
仿真结果:
仿真代码如下:
clear;
clc;
N=300;
t=0:0.01:0.01*N;
Lamda=0.4;
Qg=0.03;
Qe1=0.001;
Qe2=0.002;
Qe3=0.003;
RandnumBt=rand(1,N);
for i=1:N
if RandnumBt(i)<Lamda
RandnumBt(i)=1;
else
RandnumBt(i)=0;
end
end
RandNum=normrnd(0,sqrt(Qg),1,N);
for i=1:N
k=1+0.1*cos(2*pi*t(i)/200);
W(i)=k*RandNum(i)*RandnumBt(i);
Qw(i)=k^2*Qg*Lamda;
end
RandNumE1=normrnd(0,sqrt(Qe1),1,N);
RandNumE2=normrnd(0,sqrt(Qe2),1,N);
RandNumE3=normrnd(0,sqrt(Qe3),1,N);
x(:,1)=[0;0];
v1(1)=RandNumE1(1);
v2(1)=RandNumE2(1);
v3(1)=RandNumE3(1);
z1(:,1)=v1(1);
z2(:,1)=v2(1);
z3(:,1)=v3(1);
A1=0.001+0.0005*cos(2*pi*t(1)/100);
A2=0.002+0.0005*cos(2*pi*t(1)/100);
A3=0.003+0.0005*cos(2*pi*t(1)/100);
A=[0.1 0.25;0.5+0.1*cos(2*pi*t(1)/200) 0];
F=[2*cos(2*pi*t(1)/200);1];
for i=2:N
H=[0.3+0.1*cos(2*pi*t(i)/200) 0];
x(:,i)=A*x(:,i-1)+F*W(i-1);
%%测量系统1
v1(i)=A1*v1(i-1)+RandNumE1(i-1);
z1(:,i)=H*x(:,i)+v1(i);
y1(:,i-1)=z1(:,i)-A1*z1(:,i-1);
%%测量系统2
v2(i)=A2*v2(i-1)+RandNumE2(i-1);
z2(:,i)=H*x(:,i)+v2(i);
y2(:,i-1)=z2(:,i)-A2*z2(:,i-1);
%%测量系统3
v3(i)=A3*v3(i-1)+RandNumE3(i-1);
z3(:,i)=H*x(:,i)+v3(i);
y3(:,i-1)=z3(:,i)-A3*z3(:,i-1);
%%系统参数更新
%%系统1
Qv1(i-1)=H*F*Qw(i-1)*F'*H'+Qe1;
S1(i-1)=Qw(i-1)*F'*H';
Qw1(i-1)=Qw(i-1);
%%系统2
Qv2(i-1)=H*F*Qw(i-1)*F'*H'+Qe2;
S2(i-1)=Qw(i-1)*F'*H';
Qw2(i-1)=Qw(i-1);
%%系统3
Qv3(i-1)=H*F*Qw(i-1)*F'*H'+Qe3;
S3(i-1)=Qw(i-1)*F'*H';
Qw3(i-1)=Qw(i-1);
A1=0.001+0.0005*cos(2*pi*t(i)/100);
A2=0.002+0.0005*cos(2*pi*t(i)/100);
A3=0.003+0.0005*cos(2*pi*t(i)/100);
A=[0.1 0.25;0.5+0.1*cos(2*pi*t(i)/200) 0];
F=[2*cos(2*pi*t(i)/200);1];
end
%%卡尔曼滤波
Xk1(:,1)=[0;0];
Pkk1=[Qg,0;0,Qe1];
Xk2(:,1)=[0;0];
Pkk2=[Qg,0;0,Qe2];
Xk3(:,1)=[0;0];
Pkk3=[Qg,0;0,Qe3];
I=eye(2);
error=zeros(6,N);
error1=zeros(6,N);
P12=[Qg,0;0,Qe2];
P13=[Qg,0;0,Qe3];
P21=[Qg,0;0,Qe2];
P23=[Qg,0;0,Qe1];
P31=[Qg,0;0,Qe3];
P32=[Qg,0;0,Qe1];
e=[1;1;1];
en=[I;I;I];
m=zeros(2,2);
for i=2:N
H=[0.3+0.1*cos(2*pi*t(i-1)/200) 0];
Hn=[0.3+0.1*cos(2*pi*t(i)/200) 0];
A=[0.1 0.25;0.5+0.1*cos(2*pi*t(i)/200) 0];
A1=0.001+0.0005*cos(2*pi*t(i)/100);
A2=0.002+0.0005*cos(2*pi*t(i)/100);
A3=0.003+0.0005*cos(2*pi*t(i)/100);
%%系统1滤波
H1=Hn*A-A1*H;
Kf1(:,i-1)=Pkk1*H1'/(H1*Pkk1*H1'+Qv1(i-1));
X1(:,i-1)=Xk1(:,i-1)+Kf1(:,i-1)*(y1(i-1)-H1*Xk1(:,i-1));
P1=(I-Kf1(:,i-1)*H1)*Pkk1;
Kp1(:,i-1)=(A*Pkk1*H1'+F*S1(i-1))/(H1*Pkk1*H1'+Qv1(i-1));
Xk1(:,i)=A*Xk1(:,i-1)+Kp1(:,i-1)*(y1(i-1)-H1*Xk1(:,i-1));
Pkk1=A*Pkk1*A'-Kp1(:,i-1)*(A*Pkk1*H1'+F*S1(i-1))'+F*Qw(i-1)*F';
error(1,i)=error(1,i-1)+((X1(1,i-1)-x(1,i-1))^2-error(1,i-1))/i;
error1(1,i)=error(1,i-1)+((X1(2,i-1)-x(2,i-1))^2-error(1,i-1))/i;
%%系统2滤波
H2=Hn*A-A2*H;
Kf2(:,i-1)=Pkk2*H2'/(H2*Pkk2*H2'+Qv2(i-1));
X2(:,i-1)=Xk2(:,i-1)+Kf2(:,i-1)*(y2(i-1)-H2*Xk2(:,i-1));
P2=(I-Kf2(:,i-1)*H2)*Pkk2;
Kp2(:,i-1)=(A*Pkk2*H2'+F*S2(i-1))/(H2*Pkk2*H2'+Qv2(i-1));
Xk2(:,i)=A*Xk2(:,i-1)+Kp2(:,i-1)*(y2(i-1)-H2*Xk2(:,i-1));
Pkk2=A*Pkk2*A'-Kp2(:,i-1)*(A*Pkk2*H2'+F*S2(i-1))'+F*Qw(i-1)*F';
error(2,i)=error(2,i-1)+((X2(1,i-1)-x(1,i-1))^2-error(2,i-1))/i;
error1(2,i)=error(2,i-1)+((X2(2,i-1)-x(2,i-1))^2-error(2,i-1))/i;
%%系统3滤波
H3=Hn*A-A3*H;
Kf3(:,i-1)=Pkk3*H3'/(H3*Pkk3*H3'+Qv3(i-1));
X3(:,i-1)=Xk3(:,i-1)+Kf3(:,i-1)*(y3(i-1)-H3*Xk3(:,i-1));
P3=(I-Kf3(:,i-1)*H3)*Pkk3;
Kp3(:,i-1)=(A*Pkk3*H3'+F*S3(i-1))/(H3*Pkk3*H3'+Qv3(i-1));
Xk3(:,i)=A*Xk3(:,i-1)+Kp3(:,i-1)*(y3(i-1)-H2*Xk3(:,i-1));
Pkk3=A*Pkk3*A'-Kp3(:,i-1)*(A*Pkk3*H3'+F*S3(i-1))'+F*Qw(i-1)*F';
error(3,i)=error(3,i-1)+((X3(1,i-1)-x(1,i-1))^2-error(3,i-1))/i;
error1(3,i)=error(3,i-1)+((X3(2,i-1)-x(2,i-1))^2-error(3,i-1))/i;
%%三种导航融合
Phip1=(I-Kf1(:,i-1)*H1)*A;
Phip2=(I-Kf2(:,i-1)*H2)*A;
Phip3=(I-Kf3(:,i-1)*H3)*A;
P12=Phip1*P12*Phip2+(I-Kf1(:,i-1)*H1)*F*Qw(i-1)*F'*(I-Kf2(:,i-1)*H2)';
P13=Phip1*P13*Phip3+(I-Kf1(:,i-1)*H1)*F*Qw(i-1)*F'*(I-Kf3(:,i-1)*H3)';
P21=Phip2*P21*Phip1+(I-Kf2(:,i-1)*H2)*F*Qw(i-1)*F'*(I-Kf1(:,i-1)*H1)';
P23=Phip2*P23*Phip3+(I-Kf2(:,i-1)*H2)*F*Qw(i-1)*F'*(I-Kf3(:,i-1)*H3)';
P31=Phip3*P31*Phip1+(I-Kf3(:,i-1)*H3)*F*Qw(i-1)*F'*(I-Kf1(:,i-1)*H1)';
P32=Phip3*P32*Phip2+(I-Kf3(:,i-1)*H3)*F*Qw(i-1)*F'*(I-Kf2(:,i-1)*H2)';
%Ptr=[trace(P1) trace(P12) trace(P12);trace(P21) trace(P2) trace(P23);trace(P31) trace(P32) trace(P3)];
Ptr=[trace(P1) 0 0;0 trace(P2) 0;0 0 trace(P3)];
%%按照标量加权线性最小方差融合
a=e'/ Ptr/(e'/Ptr*e);
X4(:,i-1)=a(1)*X1(:,i-1)+a(2)*X2(:,i-1)+a(3)*X3(:,i-1);
error(4,i)=error(4,i-1)+((X4(1,i-1)-x(1,i-1))^2-error(4,i-1))/i;
error1(4,i)=error1(4,i-1)+((X4(2,i-1)-x(2,i-1))^2-error1(4,i-1))/i;
%%按照对角阵加权最小方差融合
Ptr1=[P1(1,1) 0 0;0 P2(1,1) 0;0 0 P3(1,1)];
a1=e'/ Ptr1/(e'/Ptr1*e);
X5(1,i-1)=a1(1)*X1(1,i-1)+a1(2)*X2(1,i-1)+a1(3)*X3(1,i-1);
Ptr1=[P1(2,2) 0 0;0 P2(2,2) 0;0 0 P3(2,2)];
a1=e'/ Ptr1/(e'/Ptr1*e);
X5(2,i-1)=a1(1)*X1(2,i-1)+a1(2)*X2(2,i-1)+a1(3)*X3(2,i-1);
error(5,i)=error(5,i-1)+((X5(1,i-1)-x(1,i-1))^2-error(5,i-1))/i;
error1(5,i)=error1(5,i-1)+((X5(2,i-1)-x(2,i-1))^2-error1(5,i-1))/i;
%%按照矩阵加权线性最小融合
% Pn=[P1,m,m;m,P2,m;m,m,P3];
% An=inv(en'/Pn*en)*en'/Pn
Pa=inv(inv(P1)+inv(P2)+inv(P3));
a1=Pa/P1;
a2=Pa/P2;
a3=Pa/P3;
% X6(:,i-1)=An(1:2,1:2)*X1(:,i-1)+An(1:2,3:4)*X2(:,i-1)+An(1:2,5:6)*X3(:,i-1);
X6(:,i-1)=a1*X1(:,i-1)+a2*X2(:,i-1)+a3*X3(:,i-1);
error(6,i)=error(6,i-1)+((X6(1,i-1)-x(1,i-1))^2-error(6,i-1))/i;
error1(6,i)=error1(6,i-1)+((X6(2,i-1)-x(2,i-1))^2-error1(6,i-1))/i;
end
t=1:N-1;
figure(1);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X1(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器1融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X1(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器1融合结果示意图');
figure(2);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X2(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器2融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X2(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器2融合结果示意图');
figure(3);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X3(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器3融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X3(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器3融合结果示意图');
figure(4);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X4(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('3传感器状态值1数据融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X4(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('3传感器状态值2数据融合结果示意图');
figure(5);
subplot(2,2,1);
plot(t,error(1,1:299),'r',t,error(2,1:299),'b',t,error(3,1:299),'g',t,error(4,1:299),'k',t,error(5,1:299),'m',t,error(6,1:299),'y');
legend('系统1误差','系统2误差','系统3误差','系统4误差','系统5误差','系统6误差');
title('状态值1误差示意图示意图');
subplot(2,2,2);
plot(t,error1(1,1:299),'r',t,error1(2,1:299),'b',t,error1(3,1:299),'g',t,error1(4,1:299),'k',t,error1(5,1:299),'m',t,error1(6,1:299),'y');
legend('系统1误差','系统2误差','系统3误差','系统4误差','系统5误差','系统6误差');
title('状态值2误差示意图示意图');
subplot(2,2,3);
plot(t,error(1,1:299),'r',t,error(4,1:299),'k',t,error(5,1:299),'b',t,error(6,1:299),'g');
legend('系统1误差','系统4误差','系统5误差','系统6误差');
title('状态值1误差示意图示意图');
subplot(2,2,4);
plot(t,error1(1,1:299),'r',t,error1(4,1:299),'k',t,error1(5,1:299),'b',t,error1(6,1:299),'g');
legend('系统1误差','系统4误差','系统5误差','系统6误差');
title('状态值2误差示意图示意图');
文章浏览阅读2.4w次,点赞94次,收藏388次。你认识静态链表吗?听起来是不是很陌生呢?本文将较为详细的向你介绍它,感兴趣的话就一起来看看吧。_c语言实现静态链表
文章浏览阅读211次。1.简介加密方法分为对称加密和非对称加密,区别在于对称加密只有有一个密钥,而非对称加密有两个密钥对称加解密:enccrypt(明文,密钥)=密文,decrypt(密文,密钥)=明文非对称加解密:encrypt(明文,公钥)=密文,decrypt(密文,私钥)=明文2.密钥2.1.密钥格式2.1.1.der.der密钥以二进制编码,在JAVA中需要以流的形式读取密钥。下面..._pbkdf2withhmacsha1是对称加密吗
文章浏览阅读1.7k次,点赞2次,收藏7次。libharu编译过程libharu编译过程用cmake编译zlib-1.2.11cmake编译lpngcmake编译libharulibharu编译过程看了网上关于libharu的编译过程,试了一下,都没成功,于是我自己动手,折腾一下,结果弄出来了,所以记录一下。首先,解压上面的文件夹,得到haru文件夹,里面的内容如下所示:用cmake编译zlib-1.2.11先在zlib-1.2.11下建立一个build文件夹:双击cmake-gui.exe文件:按照上图设置好路径(应该看的
文章浏览阅读1.7w次,点赞14次,收藏74次。Framework Env name (--env parameter) Description Docker Image Packages and Nvidia SettingsTensorFlow 2.2 tensorflow-2.2 TensorFlow 2.2.0 + Keras 2.3.1 on Python 3.7. floydhub/tensorflow TensorFlow-2.2TensorFlow 2.1 tensorflow-2.1 TensorFlow 2.1.0 + Kera._torch1.12.1对应的tensflow
文章浏览阅读505次。首先看看代码 @property(nonatomic,strong)NSString*id;@property(nonatomic,strong)NSString*screen_name;@property(nonatomic,strong)NSString*url..._ios 字典转模型转id
文章浏览阅读7.5k次,点赞11次,收藏77次。Matlab基础代码教程第1部分:变量定义和基本运算:% 生成矩阵 % 直接法 a = [1,2,3;4,5,6;7,8,9]; % 冒号一维矩阵 a = 开始:步长:结束,步长为1可省略 b = 1:1:10; % 1,2,...10 b = 1:10; %与上一个等价 % 函数生成 % linspace(开始,结束,元素个数),等差生成指定元素数的一维矩阵,省略个数则生成100个 c _matlab pinv的c代码
文章浏览阅读548次。1、 安装软件包:[root@master ~]# mount /dev/cdrom /media/cdrom/mount: block device /dev/cdrom is write-protected, mounting read-only[root@master ~]# cd /media/cdrom/RedHat/RPMS/[root@master RPMS]# rpm -ivh b...
文章浏览阅读472次。留存备用笔记,有错误请指出_jenkins gitlab ubuntu
文章浏览阅读1.2k次。src=http---inews.gtimg.com-newsapp_match-0-12067351979-0.jpg&refer=http---inews..jpeg (134.17 KB, 下载次数: 0)2021-3-30 14:12 上传在聊鸿蒙系统前,先简单了解下电脑及手机的主流系统。看完后,或许就能明白系统的普及之路有多漫长。Windows系统目前PC市场份额最大。优点主要是..._对鸿蒙系统的认识和理解
文章浏览阅读1.5k次。在 MySQL 中,有三种主要的类型:Text(文本)、Number(数字)和 Date/Time(日期/时间)类型。Text 类型:数据类型描述CHAR(size)保存固定长度的字符串(可包含字母、数字以及特殊字符)。在括号中指定字符串的长度。最多 255 个字符。VARCHAR(size)保存可变长度的字符串(可包含字母、数字以及特殊字符)。在括号中指定字符串的最大长度。最多 255 个字符。..._名字用什么类型mysql
文章浏览阅读549次。带宽管理: TC中规定描述带宽: mbps = 1024 kbps = 1024 * 1024 bps => byte/s mbit = 1024 kbit => kilo bit/s mb = 1024 kb = 1024 * 1024 b =>byte mbit = 1024kbit => kilo bit. 默认: 数字使用bps..._prio队列对默认数据的处理
文章浏览阅读499次。import org.opencv.core.Core;import org.opencv.core.Mat;import org.opencv.core.Point;import org.opencv.core.Scalar;import org.opencv.imgcodecs.Imgcodecs;import org.opencv.imgproc.Imgproc;//霍夫变..._java 霍夫圆定位查找印章