数据融合matlab

将多传感器信息源的数据和信息加以联合、相关及组合，获得更为精确的位置估计及位置估计。

close all;
clear all;
%%  真实轨迹模拟
kx = .01;   ky = .05;       % 阻尼系数
g = 9.8;                    % 重力
t = 15;                     % 仿真时间
Ts = 0.1;                   % 采样周期 
len = fix（t/Ts）;            % 仿真步数
dax = 3; day = 3;       % 系统噪声
X = zeros（len4）; 
X（1:） = [0 50 500 0]; % 状态模拟的初值
for k=2:len
    x = X（k-11）; vx = X（k-12）; y = X（k-13）; vy = X（k-14）; 
    x = x + vx*Ts;
    vx = vx + （-kx*vx^2+dax*randn（11））*Ts;
    y = y + vy*Ts;
    vy = vy + （ky*vy^2-g+day*randn（1））*Ts;
    X（k:） = [x vx y vy];
end
%%  构造量测量
dr = 8;  dafa = 0.1;        % 量测噪声
for k=1:len
    r = sqrt（X（k1）^2+X（k3）^2） + dr*randn（11）;
    a = atan（X（k1）/X（k3））*57.3 + dafa*randn（11）;
    Z（k:） = [r a];
end
%% ekf 滤波
Qk = diag（[0; dax/10; 0; day/10]）^2;
Rk = diag（[dr; dafa]）^2;
Pk = 10*eye（4）;
Pkk_1 = 10*eye（4）;
x_hat = [0404000]‘;
X_est = zeros（len4）;
x_forecast = zeros（41）;
z = zeros（41）;
for k=1:len
    % 1 状态预测    
    x1 = x_hat（1） + x_hat（2）*Ts;
    vx1 = x_hat（2） + （-kx*x_hat（2）^2）*Ts;
    y1 = x_hat（3） + x_hat（4）*Ts;
    vy1 = x_hat（4） + （ky*x_hat（4）^2-g）*Ts;
    x_forecast = [x1; vx1; y1; vy1];        %预测值
    % 2  观测预测
    r = sqrt（x1*x1+y1*y1）;
    alpha = atan（x1/y1）*57.3;
    y_yuce = [ralpha]‘;
    %  状态矩阵
    vx = x_forecast（2）;  vy = x_forecast（4）;
    F = zeros（44）;
    F（11） = 1;  F（12） = Ts;
    F（22） = 1-2*kx*vx*Ts;
    F（33） = 1;  F（34） = Ts;
    F（44） = 1+2*ky*vy*Ts;
    Pkk_1 = F*Pk*F‘+Qk;
    % 观测矩阵
    x = x_forecast（1）; y = x_forecast（3）;
    H = zeros（24）;
    r = sqrt（x^2+y^2）;  xy2 = 1+（x/y）^2;
    H（11） = x/r;  H（13） = y/r;
    H（21） = （1/y）/xy2;  H（23） = （-x/y^2）/xy2;
    
    Kk = Pkk_1*H‘*（H*Pkk_1*H‘+Rk）^-1;       %计算增益
    x_hat = x_forecast+Kk*（Z（k:）‘-y_yuce）;      %校正
    Pk = （eye（4）-Kk*H）*Pkk_1;
    X_est（k:） = x_hat;
end
%% 
figure hold on grid on;
plot（X（:1）X（:3）‘-b‘）;
plot（Z（:1）.*sin（Z（:2）*pi/180） Z（:1）.*cos（Z（:2）*pi/180））;
plot（X_est（:1）X_est（:3） ‘r‘）;
xlabel（‘X‘）; 
ylabel（‘Y‘）; 
title（‘EKF simulation‘）;
legend（‘real‘ ‘measurement‘ ‘ekf estimated‘）;
axis（[-5230290530]）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2227  2014-11-04 22:47  ekf.m

     文件       1476  2010-02-13 20:30  fusion.m

     文件        756  2010-02-13 20:29  kalman.m

----------- ---------  ---------- -----  ----

                 4459                    3