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

在双站定位中，EKF算法可以用于到达角定位，其基本流程如下：

1. 建立状态空间模型

$$
\begin{aligned}
x_k &= \begin{bmatrix} x \\ y \\ z \\ v_x \\ v_y \\ v_z \end{bmatrix}_k \\
F_k &= \begin{bmatrix}
1 & 0 & 0 & \Delta t & 0 & 0 \\
0 & 1 & 0 & 0 & \Delta t & 0 \\
0 & 0 & 1 & 0 & 0 & \Delta t \\
0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 1
\end{bmatrix} \\
Q_k &= \begin{bmatrix}
q_x^2 & 0 & 0 & 0 & 0 & 0 \\
0 & q_y^2 & 0 & 0 & 0 & 0 \\
0 & 0 & q_z^2 & 0 & 0 & 0 \\
0 & 0 & 0 & q_{vx}^2 & 0 & 0 \\
0 & 0 & 0 & 0 & q_{vy}^2 & 0 \\
0 & 0 & 0 & 0 & 0 & q_{vz}^2
\end{bmatrix} \\
H_k &= \begin{bmatrix}
\frac{\partial \theta_{1}}{\partial x} & \frac{\partial \theta_{1}}{\partial y} & \frac{\partial \theta_{1}}{\partial z} & 0 & 0 & 0 \\
\frac{\partial \theta_{2}}{\partial x} & \frac{\partial \theta_{2}}{\partial y} & \frac{\partial \theta_{2}}{\partial z} & 0 & 0 & 0 \\
\end{bmatrix}_k \\
R_k &= \begin{bmatrix}
\sigma_{\theta_{1}}^2 & 0 \\
0 & \sigma_{\theta_{2}}^2 \\
\end{bmatrix}
\end{aligned}
$$

其中，$x$、$y$、$z$为目标的三维坐标，$v_x$、$v_y$、$v_z$为目标的三维速度，$q_x$、$q_y$、$q_z$、$q_{vx}$、$q_{vy}$、$q_{vz}$为过程噪声方差，$\theta_1$、$\theta_2$为两个接收机接收到的到达角。

2. 初始化状态和协方差矩阵

$$
\begin{aligned}
x_0 &= \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}^T \\
P_0 &= \begin{bmatrix}
p_x^2 & 0 & 0 & 0 & 0 & 0 \\
0 & p_y^2 & 0 & 0 & 0 & 0 \\
0 & 0 & p_z^2 & 0 & 0 & 0 \\
0 & 0 & 0 & p_{vx}^2 & 0 & 0 \\
0 & 0 & 0 & 0 & p_{vy}^2 & 0 \\
0 & 0 & 0 & 0 & 0 & p_{vz}^2
\end{bmatrix}
\end{aligned}
$$

其中，$p_x$、$p_y$、$p_z$、$p_{vx}$、$p_{vy}$、$p_{vz}$为初始状态的方差。

3. 预测状态和协方差矩阵

$$
\begin{aligned}
\hat{x}_{k|k-1} &= F_k \hat{x}_{k-1|k-1} \\
P_{k|k-1} &= F_k P_{k-1|k-1} F_k^T + Q_k
\end{aligned}
$$

其中，$\hat{x}_{k|k-1}$为预测状态，$P_{k|k-1}$为预测协方差矩阵。

4. 计算卡尔曼增益

$$
\begin{aligned}
K_k &= P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}
\end{aligned}
$$

其中，$K_k$为卡尔曼增益。

5. 更新状态和协方差矩阵

$$
\begin{aligned}
\hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k(y_k - h(\hat{x}_{k|k-1})) \\
P_{k|k} &= (I - K_k H_k) P_{k|k-1}
\end{aligned}
$$

其中，$\hat{x}_{k|k}$为更新后的状态，$P_{k|k}$为更新后的协方差矩阵，$y_k$为实际观测值，$h(\hat{x}_{k|k-1})$为预测观测值。

6. 循环执行步骤3至步骤5。

下面是使用MATLAB实现双站定位的EKF算法的代码，并绘制出相应的仿真图：

clear all;clc;
% 设定参数
c = 3e8;            % 光速
f = 1575.42e6;      % 频率
lambda = c/f;       % 波长
R1 = [0 0 0]';      % 发射机1坐标
R2 = [10000 0 0]';  % 发射机2坐标
sigma_theta = 0.05; % 初始方位角误差标准差
sigma_x = 500;      % 初始位置误差标准差
sigma_v = 10;       % 初始速度误差标准差
sigma_qx = 1;       % 过程噪声x方向标准差
sigma_qy = 1;       % 过程噪声y方向标准差
sigma_qz = 1;       % 过程噪声z方向标准差
sigma_qvx = 0.1;    % 过程噪声x方向速度标准差
sigma_qvy = 0.1;    % 过程噪声y方向速度标准差
sigma_qvz = 0.1;    % 过程噪声z方向速度标准差
sigma_rx = 5;       % 观测噪声x方向标准差
sigma_ry = 5;       % 观测噪声y方向标准差
sigma_rz = 5;       % 观测噪声z方向标准差
sigma_rv = 0.1;     % 观测噪声速度标准差
dt = 0.1;           % 采样时间间隔
t = 0:dt:300;       % 时间序列
N = length(t);      % 时间序列长度

% 生成真实轨迹
x = 1000*cos(pi/50*t);
y = 1000*sin(pi/50*t);
z = 500*cos(pi/100*t);
vx = -pi/5*1000*sin(pi/50*t);
vy = pi/5*1000*cos(pi/50*t);
vz = -pi/10*500*sin(pi/100*t);

% 观测到到达角
theta1 = zeros(1, N);
theta2 = zeros(1, N);
for i = 1:N
    r1 = sqrt((x(i)-R1(1))^2+(y(i)-R1(2))^2+(z(i)-R1(3))^2);
    r2 = sqrt((x(i)-R2(1))^2+(y(i)-R2(2))^2+(z(i)-R2(3))^2);
    theta1(i) = atan2(y(i)-R1(2),x(i)-R1(1)) + asin((z(i)-R1(3))/r1);
    theta2(i) = atan2(y(i)-R2(2),x(i)-R2(1)) + asin((z(i)-R2(3))/r2);
end

% 加入噪声
theta1 = theta1 + sigma_theta*randn(1,N);
theta2 = theta2 + sigma_theta*randn(1,N);
rx = x + sigma_x*randn(1,N);
ry = y + sigma_x*randn(1,N);
rz = z + sigma_x*randn(1,N);
rvx = vx + sigma_v*randn(1,N);
rvy = vy + sigma_v*randn(1,N);
rvz = vz + sigma_v*randn(1,N);

% EKF算法
x_hat = zeros(6,N);   % 估计状态
P = zeros(6,6,N);     % 估计协方差矩阵
x_hat(:,1) = [0 0 0 0 0 0]'; % 初始状态
P(:,:,1) = diag([sigma_x^2 sigma_x^2 sigma_x^2 sigma_v^2 sigma_v^2 sigma_v^2]); % 初始协方差矩阵
for i = 2:N
    % 预测状态和协方差矩阵
    F = [1 0 0 dt 0 0;
         0 1 0 0 dt 0;
         0 0 1 0 0 dt;
         0 0 0 1 0 0;
         0 0 0 0 1 0;
         0 0 0 0 0 1];
    Q = diag([sigma_qx^2 sigma_qy^2 sigma_qz^2 sigma_qvx^2 sigma_qvy^2 sigma_qvz^2]);
    x_hat(:,i|1) = F*x_hat(:,i-1);
    P(:,:,i|1) = F*P(:,:,i-1)*F' + Q;
    
    % 计算卡尔曼增益
    H = [(rx(i)-R1(1))/sqrt((rx(i)-R1(1))^2+(ry(i)-R1(2))^2) (ry(i)-R1(2))/sqrt((rx(i)-R1(1))^2+(ry(i)-R1(2))^2) (rz(i)-R1(3))/sqrt((rx(i)-R1(1))^2+(ry(i)-R1(2))^2+(rz(i)-R1(3))^2) 0 0 0;
         (rx(i)-R2(1))/sqrt((rx(i)-R2(1))^2+(ry(i)-R2(2))^2) (ry(i)-R2(2))/sqrt((rx(i)-R2(1))^2+(ry(i)-R2(2))^2) (rz(i)-R2(3))/sqrt((rx(i)-R2(1))^2+(ry(i)-R2(2))^2+(rz(i)-R2(3))^2) 0 0 0];
    R = diag([sigma_rx^2 sigma_ry^2 sigma_rz^2 sigma_rv^2]);
    K = P(:,:,i|1)*H'/(H*P(:,:,i|1)*H'+R);
    
    % 更新状态和协方差矩阵
    y = [theta1(i);theta2(i)];
    h = [atan2(ry(i)-R1(2),rx(i)-R1(1))+asin((rz(i)-R1(3))/sqrt((rx(i)-R1(1))^2+(ry(i)-R1(2))^2+(rz(i)-R1(3))^2));
         atan2(ry(i)-R2(2),rx(i)-R2(1))+asin((rz(i)-R2(3))/sqrt((rx(i)-R2(1))^2+(ry(i)-R2(2))^2+(rz(i)-R2(3))^2))];
    x_hat(:,i) = x_hat(:,i|1) + K*(y-h);
    P(:,:,i) = (eye(6)-K*H)*P(:,:,i|1);
end

% 画图
figure(1);
plot3(rx,ry,rz,'b',x_hat(1,:),x_hat(2,:),x_hat(3,:),'r');
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');

figure(2);
subplot(311);
plot(t,x,t,x_hat(1,:),'r');
legend('真实值','估计值');
xlabel('时间');
ylabel('X');
grid on;
subplot(312);
plot(t,y,t,x_hat(2,:),'r');
legend('真实值','估计值');
xlabel('时间');
ylabel('Y');
grid on;
subplot(313);
plot(t,z,t,x_hat(3,:),'r');
legend('真实值','估计值');
xlabel('时间');
ylabel('Z');
grid on;