用EKF解决到达角定位算法，模型为双站定位二维情况，写出matlab代码并得到仿真图

首先，我们需要定义EKF的状态变量和观测变量，并初始化协方差矩阵和状态估计值。在这个例子中，状态变量为目标位置和速度，观测变量为两个基站接收到的信号到达时间差（TDOA）。

% Define state variables
x = [0; 0; 0; 0]; % [x_pos, y_pos, x_vel, y_vel]

% Define observation variables
z = [0; 0]; % [tdoa_1_2, tdoa_1_3]

% Initialize covariance matrix and state estimate
P = eye(4);
Q = diag([0.1, 0.1, 0.1, 0.1]); % Process noise covariance
R = diag([0.1, 0.1]); % Measurement noise covariance

接下来，我们需要定义状态转移函数和观测函数。在这个例子中，状态转移函数为简单的匀加速直线运动模型，观测函数为基站之间的TDOA。

% Define state transition function
dt = 0.1; % Time step
A = [1, 0, dt, 0;
     0, 1, 0, dt;
     0, 0, 1, 0;
     0, 0, 0, 1];
B = [0.5*dt^2, 0;
     0, 0.5*dt^2;
     dt, 0;
     0, dt];
u = [0; 0]; % Control input (none in this example)
f = @(x, u) A*x + B*u;

% Define observation function
c = 3e8; % Speed of light
d1 = 10; % Distance to first base station
d2 = 20; % Distance to second base station
d3 = 30; % Distance to third base station
h = @(x) [(norm(x(1:2)-[0;0])-norm(x(1:2)-[d1;0]))/c;
          (norm(x(1:2)-[0;0])-norm(x(1:2)-[0;d2]))/c;
          (norm(x(1:2)-[0;0])-norm(x(1:2)-[d3;d2]))/c];

现在我们可以开始运行EKF算法。首先，我们需要从传感器获取观测值，并将其输入到EKF中进行状态估计。然后，我们使用状态估计值来预测下一个状态，并将其与传感器观测值进行比较以更新状态估计值和协方差矩阵。

% Run EKF algorithm
for i = 1:100
    % Get observation from sensors
    z = h(x) + sqrt(R)*randn(2,1);
    
    % Predict next state and covariance
    x_pred = f(x, u);
    P_pred = A*P*A' + Q;
    
    % Calculate Kalman gain
    K = P_pred*c'*pinv(c*P_pred*c' + R);
    
    % Update state estimate and covariance
    x = x_pred + K*(z - h(x_pred));
    P = (eye(4) - K*c)*P_pred;
    
    % Plot current estimate
    plot(x(1), x(2), 'ro');
    hold on;
    axis([-50 50 -50 50]);
    xlabel('X position');
    ylabel('Y position');
    pause(0.1);
end

最后，我们可以运行仿真并绘制目标的运动轨迹。

% Simulate target motion
x_true = [0; 0; 10; 10]; % [x_pos, y_pos, x_vel, y_vel]
figure;
for i = 1:100
    % Update true state
    x_true = f(x_true, u);
    
    % Plot true state
    plot(x_true(1), x_true(2), 'bx');
    hold on;
end

以下是完整的MATLAB代码：