【信息融合】卡尔曼滤波kalman信息融合【含Matlab源码 3595期】

本文详细介绍了Matlab中卡尔曼滤波原理及其在线性与非线性系统中的应用,包括扩展卡尔曼和无迹卡尔曼,以及数据融合的不同方法。并通过代码示例展示了如何在Matlab中实现这些技术,包括噪声处理和蒙特卡洛实验的应用。
摘要由CSDN通过智能技术生成

✅博主简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,Matlab项目合作可私信。
🍎个人主页:海神之光
🏆代码获取方式:
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭:行百里者,半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理(进阶版)
路径规划(Matlab)
神经网络预测与分类(Matlab)
优化求解(Matlab)
语音处理(Matlab)
信号处理(Matlab)
车间调度(Matlab)

⛄一、 卡尔曼滤波kalman信息融合

1 kalman原理
卡尔曼滤波是一种递推式滤波方法,不须保存过去的历史信息,新数据结合前一刻已求得的估计值及系统本身的状态方程按一定方式求得新的估计值。

1.1 线性卡尔曼
假设线性系统状态是k,卡尔曼原理可用以下五个公式表达:
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (1)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量;式 (2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的 covariance,A’表示A的转置矩阵,Q是系统过程的covariance;现在状态(k)的最优化估算值为X(k|k);Kg为卡尔曼增益(Kalman Gain)。

1.2 扩展卡尔曼
实际系统总是存在不同程度的非线性,对于非线性系统滤波问题,常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题,这就是扩展Kalman 滤波方法(Extended Kalman Filter,EKF )思路。扩展Kalman 滤波建立在线性Kalman 滤波的基础上,其核心是对一般的非线性系统将滤波值非线性函数f()和h()展开成Taylor级数并略去二阶及以上项,得到一个近似的线性化模型,然后应用Kalman 滤波完成对目标的滤波估计处理。

1.3、无迹卡尔曼
扩展Kalman滤波是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项,不可避免地引入了线性化误差。如果线性化假设不成立,采用这算法则会导致滤波器性能下降以至于造成发散。无迹Kalman 滤波(Unscented Kalman Filter,UKF )摒弃了对非线性函数进行线性化的传统做法,采用Kalman 线性滤波框架,对于预测方程,使用无迹变换(Unscented Transform, UT) 来处理均值和协方差的非线性传递问题。

2 数据融合(信息融合)
将经过集成处理的多传感器信息进行合成,形成一种对外部环境或被测对象某一特征的表达方式称为信息融合。常见由以下几种信息融合方法。
(1)综合平均法,即把各传感器数据求平均,乘上权重系数;
(2)贝叶斯估计法,通过先验信息和样本信息得到后验分布,再对目标进行预测;
(3)D-S法,采用概率区间和不确定区间判定多证据体下假设的似然函数进行推理;
(4)神经网络法,以测量目标的各种参数集为神经网络的输入,输出推荐的目标特征或权重;
(5)kalman法、专家系统法等。。。

⛄二、部分源代码

warning off MATLAB:singularMatrix

sennum = 5; %传感器个数
ss = 3; %状态的维数
T = 300; %采样的次数
SampleT = 300; %时隙
init_X = [ 0; 0; 0 ]; %初值
detaw = 1; %系统噪声的方差
mcnum = 1; %蒙特卡罗实验的次数
init_V = 0.1*eye(3); %P0值设定
[m,n] = size(init_X);
StartTime = 50;%这个变量留待后面说明

norela = 1;%噪声是否相关的标记位,0为不相关,1为相关

noisew = zeros( 1, T );%为噪声矩阵、x阵、y阵、R、S、Spre开辟空间
x = zeros( ss, T );
xu = zeros(ss, T);
y = zeros( 1, T, ss);
R = zeros( 1, 1, ss);
S = zeros( 1, 1, ss);
Spre = zeros( ss, ss );

F = [1, SampleT, (SampleT^2)/2; 0, 1, SampleT; 0, 0, 1];%系统矩阵
W = [0; 0; 1];%噪声影响矩阵
Q = detaw;
L = sennum;

%detas数组是各个传感器观测噪声的方差,a数组是噪声不独立时各传感器噪声产生的权重,H是观测矩阵
detas(1) = 5;
detas(2) = 5;
detas(3) = 5;
detas(4) = 5;
detas(5) = 5;
a(1) = 0.5;
a(2) = 0.5;
a(3) = 0.5;
a(4) = 0.5;
a(5) = 0.5;
H(:,:,1) = [ 1, 0, 0 ];
H(:,:,2) = [ 0, 1, 0 ];
H(:,:,3) = [ 0, 0, 1 ];
H(:,:,4) = [ 1, 1, 0 ];
H(:,:,5) = [ 0, 1, 1 ];

if mcnum == 1%蒙特卡洛实验次数为1,不进行蒙特卡洛实验。
if norela == 1%如果噪声相关
for t = 1:T%这个循环用于产生真实轨迹和观测值
noisew(t) = detaw^0.5randn(1);%系统噪声(高斯噪声)
if t == 1
x(:,1) = init_X;%初值
xu(:,1) = init_X;
else
x(:,t) = F
x(:,t-1) + Wnoisew(t);
xu(:,1) = F
xu(:,t-1);
end
for i = 1:L%这个循环用于产生观测噪声
ipus = detas(i)^0.5randn(1);
noisev = a(i)noisew(t) + ipus;
y(:,t,i) = H(:,:,i)x(:,t) + noisev;
end
end
for i = 1:L%这两个循环用于产生S、R、Spre矩阵,这三个矩阵将用于融合过程
S(:,:,i) = a(i)detaw;
R(:,:,i) = a(i)^2
detaw + detas(i);
end
for i = 1:L
for j = 1:L
if i == j
Spre(i,i) = a(i)^2
detaw + detas(i);
else
Spre(i,j) = a(i)a(j)detaw;
end
end
end
else%如果噪声不相关
for t = 1:T%这个循环用于产生真实轨迹和观测值
noisew(t) = detaw^0.5
randn(1);
if t == 1
x(:,1) = init_X;
xu(:,1) = init_X;
else
x(:,t) = F
x(:,t-1) + W
noisew(t);
xu(:,1) = F
xu(:,t-1);
end
for i = 1:L%这个循环用于产生观测噪声
noisev = detas(i)^0.5*randn(1);
y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
end
end
for i = 1:L
S(:,:,i) = 0;
R(:,:,i) = detas(i);
end
for i = 1:L
for j = 1:L
Spre(i,j) = 0;
end
end
end

t = cputime;
[xfilt, Vfilt, VVfilt, K, J, An] = kalman_filter(y, F, H, detaw, W, S, R, init_X, init_V);%Kalman滤波
filttime = cputime - t;

t = cputime;
PreP = Pretreat(F, H, Q, K, Vfilt, J, An, Spre, S, R, W);%计算P
PrePtime = cputime - t;
[xcter1, Ptra1, wmtime] = matcen(xfilt,PreP); %矩阵
[xcter2, Ptra2, watime] = center(xfilt,Vfilt); %加权平均
[xcter3, Ptra3, wstime] = lincen(xfilt,PreP);%标量

Twa = filttime + watime;
Twm = filttime + PrePtime + wmtime;
Tws = filttime + PrePtime + wstime;
watime = 1;
wmtime = Twm/Twa;
wstime = Tws/Twa;

dfilt1 = x - xcter1;
A1 = sum(dfilt1.^2);
mse_filt1 = sqrt(sum(A1(StartTime:T)))/(T-StartTime);


dfilt2 = x - xcter2;
A2 = sum(dfilt2.^2);
mse_filt2 = sqrt(sum(A2(StartTime:T)))/(T-StartTime);


dfilt3 = x - xcter3;
A3 = sum(dfilt3.^2);
mse_filt3 = sqrt(sum(A3(StartTime:T)))/(T-StartTime);

TI = StartTime:T; 

% figure
% hold on
% plot(TI,x(finum,StartTime:T),‘-k’);
% plot(TI,xu(finum,StartTime:T),‘-r’);
% xlabel(‘采样次数’)
% ylabel(‘位移’)
% legend(‘真值’,‘矩阵加权’,2);
% hold off

for finum = 1:ss;
    figure
    hold on
    plot(TI,x(finum,StartTime:T),'-k');
    plot(TI, xcter1(finum,StartTime:T), '-r');
    plot(TI, xcter2(finum,StartTime:T), '-g');
    plot(TI, xcter3(finum,StartTime:T), '-m');
    xlabel('采样次数')
    ylabel('位移')
    help legend('真值','矩阵加权','加权平均', '标量加权',4);
    title('融合结果比较');
    hold off
end

TI = 1:T;
figure
hold on
title('融合中心P0的迹的比较');
plot(TI,Ptra1,'-r');
plot(TI,Ptra2,'-g');
plot(TI,Ptra3,'-b');
xlabel('采样次数')
ylabel('P0的迹')
help legend('矩阵加权P0的迹','加权平均P0的迹', '标量加权P0的迹',3);
hold off

for senth = 1:sennum
    figure
    hold on
    plot(TI, H(:,:,senth)*x(:,:),'-k');
    plot(TI, y(:,:,senth), 'g*');
    plot(TI, H(:,:,senth)*xfilt(:,:,senth), 'rx:');
    hold off
    help legend('真值','观测值','滤波器值',3);
    xlabel('采样次数')
    ylabel('位移')

    A = (H(:,:,senth) * ( xfilt(:,:,senth) - x(:,:) )).^2;
    AA = (y(:,:,senth) - H(:,:,senth) * x(:,:)).^2;
    BB = -30;
    figure
    hold on
    title('误差');
    xlabel('采样次数')
    ylabel('error')
    plot(TI,A,'-r');
    plot(TI,AA,'g*');
    plot(TI,BB,'-k')
    help legend('单传感器kalman滤波后误差','观测值与真值的误差',2);
    hold off
end

watime %加权平均
wmtime %矩阵加权
wstime %标量加权
wm_RMS = mse_filt1  %矩阵加权
wa_RMS = mse_filt2 %加权平均
ws_RMS = mse_filt3 %标量加权

else%蒙特卡洛实验
%蒙特卡洛实验的过程只是将上述运行一遍的过程多次运行,并将求得的结果取平均
wmT=0;
wsT=0;
wmm=0;
waa=0;
wss=0;
for ttt = 1:mcnum
if norela == 1%如果噪声相关
for t = 1:T
noisew(t) = detaw^0.5randn(1);%系统噪声(高斯噪声)
if t == 1
x(:,1) = init_X;
else
x(:,t) = F
x(:,t-1) + Wnoisew(t);
end
for i = 1:L
ipus = detas(i)^0.5
randn(1);
noisev = a(i)noisew(t) + ipus;
y(:,t,i) = H(:,:,i)x(:,t) + noisev;
end
end
for i = 1:L
S(:,:,i) = a(i)detaw;
R(:,:,i) = a(i)^2
detaw + detas(i);
end
for i = 1:L
for j = 1:L
if i == j
Spre(i,i) = a(i)^2
detaw + detas(i);
else
Spre(i,j) = a(i)a(j)detaw;
end
end
end
else%如果噪声不相关
for t = 1:T
noisew(t) = detaw^0.5
randn(1);
if t == 1
x(:,1) = init_X;
else
x(:,t) = F
x(:,t-1) + W
noisew(t);
end
for i = 1:L
noisev = detas(i)^0.5*randn(1);
y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
end
end
for i = 1:L
S(:,:,i) = 0;
R(:,:,i) = detas(i);
end
for i = 1:L
for j = 1:L
Spre(i,j) = 0;
end
end
end

    t = cputime;
    [xfilt, Vfilt, VVfilt, K, J, An] = kalman_filter(y, F, H, detaw, W, S, R, init_X, init_V);
    filttime = cputime - t;

    t = cputime;
    PreP = Pretreat(F, H, Q, K, Vfilt, J, An, Spre, S, R, W);%计算P
    PrePtime = cputime - t;
    [xcter1, Ptra1, wmtime] = matcen(xfilt,PreP); %矩阵
    [xcter2, Ptra2, watime] = center(xfilt,Vfilt); %加权平均
    [xcter3, Ptra3, wstime] = lincen(xfilt,PreP);%标量

    Twa = filttime + watime;
    Twm = filttime + PrePtime + wmtime;
    Tws = filttime + PrePtime + wstime;
    watime = 1;
    wmtime = Twm/Twa;
    wstime = Tws/Twa;
    wmT = wmT + wmtime/mcnum;
    wsT = wsT + wstime/mcnum;

    dfilt1 = x - xcter1;
    A1 = sum(dfilt1.^2);
    wm_RMS = sqrt(sum(A1(StartTime:T)))/(T-StartTime);
    wmm = wmm + wm_RMS/mcnum;
    %mse_filt1 = sqrt(sum(A1));


    dfilt2 = x - xcter2;
    A2 = sum(dfilt2.^2);
    wa_RMS = sqrt(sum(A2(StartTime:T)))/(T-StartTime);
    waa = waa + wa_RMS/mcnum;
    %mse_filt2 = sqrt(sum(A2));


    dfilt3 = x - xcter3;
    A3 = sum(dfilt3.^2);
    ws_RMS = sqrt(sum(A3(StartTime:T)))/(T-StartTime);
    wss = wss + ws_RMS/mcnum;
    %mse_filt3 = sqrt(sum(A3));

end
wmm
waa
wss
wmT
wsT

end

⛄三、运行结果

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]陈寅,林良明,颜国正.D-S证据推理在信息融合应用中的存在问题及改进[J].系统工程与电子技术. 2000,(11)

[2]邓肯·麦克尼尔.多传感器数据融合:基于卡尔曼滤波和神经网络的方法[M].机械工业出版社, 2005

3 备注
简介此部分摘自互联网,仅供参考,若侵权,联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络(CNN)、LSTM、支持向量机(SVM)、最小二乘支持向量机(LSSVM)、极限学习机(ELM)、核极限学习机(KELM)、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题(TSP)、车辆路径问题(VRP、MVRP、CVRP、VRPTW等)、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值