无人机通过测量与两个已知位置锚点的角度通过EKF方法，写出matlab 代码来估计自身位置，并做出估计轨迹和真实轨迹图

以下是一个简单的Matlab代码，用于估计无人机的位置，并绘制估计轨迹和真实轨迹图。这个例子中，我们假设无人机通过测量与两个已知位置锚点的角度，然后使用扩展卡尔曼滤波器（EKF）估计无人机的位置。

%定义初始位置和速度
x = [0; 0; 0; 10; 10];

%定义状态转移矩阵和过程噪声
A = [eye(3), eye(3); zeros(3), eye(3)];
Q = diag([0.01, 0.01, 0.01, 0.01, 0.01, 0.01]);

%定义观测矩阵和观测噪声
H = [eye(2), zeros(2, 4)];
R = diag([0.1, 0.1]);

%定义锚点位置
anchor1 = [0, 0];
anchor2 = [10, 0];

%定义真实轨迹和观测值
N = 100;
true_trajectory = zeros(3, N);
measurements = zeros(2, N);

for i = 1:N
    %生成真实轨迹
    true_trajectory(:, i) = A * x;
    %生成观测值
    measurements(:, i) = [atan2(x(2)-anchor1(2), x(1)-anchor1(1)); ...
        atan2(x(2)-anchor2(2), x(1)-anchor2(1))] + normrnd(0, R, [2, 1]);
    %使用EKF进行估计
    [x, P] = ekf(x, P, measurements(:, i), A, Q, H, R);
end

%绘制真实轨迹和估计轨迹
figure
plot(true_trajectory(1,:), true_trajectory(2,:), 'r');
hold on;
plot(x(1,:), x(2,:), 'b');
title('True and Estimated Trajectories');
xlabel('X position');
ylabel('Y position');
legend('True Trajectory', 'Estimated Trajectory');

其中，`ekf`函数是一个用于实现扩展卡尔曼滤波器的自定义函数。这里我们假设你已经实现了这个函数。通过运行这个代码，你应该可以得到一个轨迹图，其中真实轨迹用红色表示，估计轨迹用蓝色表示。