kalman滤波融合原理及其matlab仿真「建议收藏」

大家好，又见面了，我是你们的朋友全栈君。

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.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法、专家系统法等。。。

3、matlab仿真

本文实现对两路卫星定位数据（可理解为一路GPS数据，一路BD数据)，首先进行kalman滤波，再进行信息融合，得到更加平稳且误差小的定位结果。

（1）两路数据，以.txt文件输入

（2）获取数据

clc;
clear all           
longitude(1)=0;
latitude(1)=0;
y(1)=119.12256;

fid=fopen('.\test1.txt');  %打开数据总文件
B=textscan(fid,'%f %f');%把每一列的数据读入到读入到单元数组B中
C=[B{1} B{2}];          %从单元数组B中提取每列数据赋值给矩阵C
n=size(C,1);         %确定读入数据的坐标数目
longitude=C(:,1);latitude=C(:,2);      %赋值
fclose(fid);

fod=fopen('.\test2.txt');  %打开数据总文件
A=textscan(fod,'%f %f');%把每一列的数据读入到读入到单元数组B中
G=[A{1} A{2}];          %从单元数组B中提取每列数据赋值给矩阵C
num=size(G,1);         %确定读入数据的坐标数目
longitude2=G(:,1);latitude2=G(:,2);      %赋值
fclose(fod);

（3）kalman处理

longitude_fil(1)=y(1);
p_fil(1)=1;
for t=2:n;
    longitude_pre(t)=a*longitude_fil(t-1);
    p_pre(t)=a*p_fil(t-1)*a'+P;
    K(t)=p_pre(t)*H'*inv(H*p_pre(t)*H'+Q);
    longitude_fil(t)=longitude_pre(t)+K(t)*(longitude(t)-H*longitude_pre(t));
    p_fil(t)=(1-K(t)*H)*p_pre(t);
end

for i=1:n
     Err1(i)=RMS(y(i),longitude(i));
    Err2(i)=RMS(y(i),longitude_fil(i));
end

（4）融合处理

w1=var(longitude_fil-y);
w2=var(longitude2_fil-j);
disp('w1=');disp(w1);
disp('w2=');disp(w2);
result=(w2/(w1+w2))*longitude_fil+(w1/(w1+w2))*longitude2_fil;
w3=var(result);
disp('w3=');disp(w3);
disp('result=');disp(result);
for i=1:num;

dlmwrite('.\result.txt',result,'delimiter','%10.000f\t');

end 

figure(7);
hold on;
plot(1:num,longitude_fil,'-+k');
plot(1:num,longitude2_fil,'-*b');
plot(1:num,result,'-og');
%plot(1:n,result,'-*r');
hold off;
legend('滤波后1','滤波后2','融合后');
xlabel('采样次数','color','b');
ylabel('幅值','color','b');
title('坐标图线','color','m');

（5）滤波前后误差