卡尔曼滤波实现数据融合matlab

卡尔曼滤波是一种线性高斯滤波器，是一种常用的数据融合方法。在Matlab中实现卡尔曼滤波的步骤如下：

1. 定义卡尔曼滤波器的状态变量、状态转移矩阵、观测矩阵、观测噪声矩阵、状态噪声矩阵和初始状态。

% 定义状态变量
X = [x; y; vx; vy]; 
% 定义状态转移矩阵
F = [1 0 dt 0;
     0 1 0 dt;
     0 0 1 0;
     0 0 0 1];
% 定义观测矩阵
H = [1 0 0 0;
     0 1 0 0];
% 定义观测噪声矩阵
R = [varx 0;
     0 vary];
% 定义状态噪声矩阵
Q = [varvx 0 0 0;
     0 varvy 0 0;
     0 0 varax 0;
     0 0 0 varay];
% 定义初始状态
X0 = [x0; y0; vx0; vy0];

2. 对于每一个时刻，通过状态转移矩阵预测状态，并更新卡尔曼增益、估计误差协方差矩阵和估计状态。

% 预测状态
Xp = F*X;
% 预测误差协方差矩阵
Pp = F*P*F'+Q;
% 计算卡尔曼增益
K = Pp*H'/(H*Pp*H'+R);
% 更新状态
X = Xp + K*([xobs; yobs]-H*Xp);
% 更新误差协方差矩阵
P = (eye(4)-K*H)*Pp;

其中，`xobs`和`yobs`是观测到的位置坐标，`P`是估计误差协方差矩阵。

3. 重复步骤2直到观测结束。

完整的Matlab代码实现如下：

% 定义参数
dt = 0.1; % 时间间隔
x0 = 0; % 初始位置
y0 = 0;
vx0 = 10; % 初始速度
vy0 = 10;
varx = 0.1; % 观测噪声方差
vary = 0.1;
varvx = 0.01; % 状态噪声方差
varvy = 0.01;
varax = 0.01;
varay = 0.01;

% 定义状态变量
X = [x0; y0; vx0; vy0];
% 定义状态转移矩阵
F = [1 0 dt 0;
     0 1 0 dt;
     0 0 1 0;
     0 0 0 1];
% 定义观测矩阵
H = [1 0 0 0;
     0 1 0 0];
% 定义观测噪声矩阵
R = [varx 0;
     0 vary];
% 定义状态噪声矩阵
Q = [varvx 0 0 0;
     0 varvy 0 0;
     0 0 varax 0;
     0 0 0 varay];
% 定义初始状态
X0 = [x0; y0; vx0; vy0];
% 定义初始误差协方差矩阵
P = eye(4);

% 生成随机运动轨迹
t = 0:dt:10;
xtrue = x0 + vx0*t;
ytrue = y0 + vy0*t;
vxtrue = vx0*ones(size(t));
vytrue = vy0*ones(size(t));
xobs = xtrue + sqrt(varx)*randn(size(t));
yobs = ytrue + sqrt(vary)*randn(size(t));

% 进行卡尔曼滤波
X = X0;
for i=1:length(t)
    % 预测状态
    Xp = F*X;
    % 预测误差协方差矩阵
    Pp = F*P*F'+Q;
    % 计算卡尔曼增益
    K = Pp*H'/(H*Pp*H'+R);
    % 更新状态
    X = Xp + K*([xobs(i); yobs(i)]-H*Xp);
    % 更新误差协方差矩阵
    P = (eye(4)-K*H)*Pp;
end

% 绘图
figure;
plot(xtrue,ytrue,'k-',xobs,yobs,'ro',X(1),X(2),'bx');
legend('真实轨迹','观测值','卡尔曼估计');
xlabel('x');
ylabel('y');