简化的 JPDA 算法实现

一、算法核心思想

JPDA 解决的是 "一个目标可能对应多个观测" 的数据关联问题，通过计算每个观测来自每个目标的概率，然后加权平均更新目标状态。

---

二、MATLAB 实现

1、主测试程序

%% 简化 JPDA 算法演示
clc; clear; close all;

% 参数设置
dt = 1;          % 时间步长
N = 50;         % 总步数
n_targets = 2;  % 目标数
PD = 0.9;       % 检测概率
PG = 0.99;      % 门概率
lambda = 0.1;   % 杂波密度
gate_threshold = 9.21;  % 卡方门限（95%置信度）

% 目标初始状态
x_true = zeros(4, n_targets, N);
x_est = zeros(4, n_targets, N);
P_est = zeros(4, 4, n_targets, N);

% 初始化
x_true(:,:,1) = [0, 10; 1, 0; 0, 10; 0, 1];
x_est(:,:,1) = x_true(:,:,1) + randn(4, n_targets)*0.5;
for m=1:n_targets
    P_est(:,:,m,1) = eye(4);
end

% 系统模型
A = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1];
C = [1 0 0 0; 0 0 1 0];
Q = 0.01 * eye(4);
R = 0.1 * eye(2);

% 存储测量
measurements = cell(1, N);

%% 主循环
for k = 2:N
    fprintf('Step %d/%d\n', k, N);
    
    % 1. 目标真实运动
    for m = 1:n_targets
        x_true(:,m,k) = A * x_true(:,m,k-1) + sqrt(Q) * randn(4,1);
    end
    
    % 2. 生成观测（含杂波）
    Z = [];
    
    % 2.1 真实目标观测
    for m = 1:n_targets
        if rand < PD
            z_true = C * x_true(:,m,k) + sqrt(R) * randn(2,1);
            Z = [Z, z_true];
        end
    end
    
    % 2.2 生成杂波
    n_clutter = poissrnd(lambda);
    for i = 1:n_clutter
        z_clutter = 20*rand(2,1) - 10;  % 在区域内随机生成杂波
        Z = [Z, z_clutter];
    end
    
    measurements{k} = Z;
    
    % 3. 预测步骤
    x_pred = zeros(4, n_targets);
    P_pred = zeros(4, 4, n_targets);
    
    for m = 1:n_targets
        x_pred(:,m) = A * x_est(:,m,k-1);
        P_pred(:,:,m) = A * P_est(:,:,m,k-1) * A' + Q;
    end
    
    % 4. 简化 JPDA 更新
    for m = 1:n_targets
        [x_upd, P_upd] = simplified_jpda_update(...
            x_pred(:,m), P_pred(:,:,m), Z, C, R, PD, PG, lambda, gate_threshold);
        
        x_est(:,m,k) = x_upd;
        P_est(:,:,m,k) = P_upd;
    end
end

%% 可视化
figure('Position', [100, 100, 1200, 500]);

% 子图1：目标1轨迹
subplot(1,2,1);
hold on; grid on;
plot(squeeze(x_true(1,1,:)), squeeze(x_true(3,1,:)), 'b-', 'LineWidth', 2);
plot(squeeze(x_est(1,1,:)), squeeze(x_est(3,1,:)), 'r--', 'LineWidth', 1.5);

% 绘制测量点
for k = 1:N
    if ~isempty(measurements{k})
        scatter(measurements{k}(1,:), measurements{k}(2,:), 20, 'k', 'filled');
    end
end

xlabel('X 位置'); ylabel('Y 位置');
title('目标1：真实轨迹 vs JPDA估计');
legend('真实轨迹', 'JPDA估计', '观测点');
axis equal;

% 子图2：目标2轨迹
subplot(1,2,2);
hold on; grid on;
plot(squeeze(x_true(1,2,:)), squeeze(x_true(3,2,:)), 'g-', 'LineWidth', 2);
plot(squeeze(x_est(1,2,:)), squeeze(x_est(3,2,:)), 'm--', 'LineWidth', 1.5);

for k = 1:N
    if ~isempty(measurements{k})
        scatter(measurements{k}(1,:), measurements{k}(2,:), 20, 'k', 'filled');
    end
end

xlabel('X 位置'); ylabel('Y 位置');
title('目标2：真实轨迹 vs JPDA估计');
legend('真实轨迹', 'JPDA估计', '观测点');
axis equal;

%% 性能评估
% 计算均方根误差
rmse = zeros(n_targets, 1);
for m = 1:n_targets
    pos_error = squeeze(x_est([1,3],m,:) - x_true([1,3],m,:));
    rmse(m) = sqrt(mean(sum(pos_error.^2, 1)));
end

fprintf('\n=== 性能评估 ===\n');
for m = 1:n_targets
    fprintf('目标%d RMSE: %.4f\n', m, rmse(m));
end

2、简化 JPDA 更新函数

function [x_upd, P_upd] = simplified_jpda_update(...
    x_pred, P_pred, Z, C, R, PD, PG, lambda, gate_threshold)
% 简化 JPDA 更新函数
% 输入：
%   x_pred: 预测状态 [4x1]
%   P_pred: 预测协方差 [4x4]
%   Z: 观测集合 [2 x n_obs]
%   C: 观测矩阵 [2x4]
%   R: 观测噪声协方差 [2x2]
%   PD: 检测概率
%   PG: 门概率
%   lambda: 杂波密度
%   gate_threshold: 门限
% 输出：
%   x_upd: 更新后的状态
%   P_upd: 更新后的协方差

n_obs = size(Z, 2);

% 如果没有观测，使用预测值
if n_obs == 0
    x_upd = x_pred;
    P_upd = P_pred;
    return;
end

% 1. 计算新息协方差
S = C * P_pred * C' + R;
S_inv = inv(S);
det_S = det(S);

% 2. 计算卡尔曼增益
K = P_pred * C' / S;

% 3. 计算每个观测的关联概率
beta = zeros(1, n_obs);  % 关联概率
d2 = zeros(1, n_obs);    % 马氏距离平方

for j = 1:n_obs
    % 新息
    innov = Z(:,j) - C * x_pred;
    
    % 马氏距离
    d2(j) = innov' * S_inv * innov;
    
    % 门检查
    if d2(j) <= gate_threshold
        % 高斯似然
        beta(j) = exp(-0.5 * d2(j)) / sqrt((2*pi)^2 * det_S);
    end
end

% 4. 加入漏检项（目标未被检测到的概率）
beta_0 = 1 - PD * PG;  % 漏检概率
beta_total = sum(beta) + beta_0 + lambda;

% 5. 归一化概率
if beta_total > 0
    beta = beta / beta_total;
    beta_0 = beta_0 / beta_total;
else
    beta = zeros(1, n_obs);
    beta_0 = 1;
end

% 6. 计算加权新息
weighted_innov = zeros(4, 1);
for j = 1:n_obs
    if beta(j) > 0
        innov = Z(:,j) - C * x_pred;
        weighted_innov = weighted_innov + beta(j) * K * innov;
    end
end

% 7. 状态更新
x_upd = x_pred + weighted_innov;

% 8. 协方差更新（简化版）
P_c = (1 - beta_0) * (P_pred - K * S * K');
P_upd = beta_0 * P_pred + P_c;

% 稳定性检查
P_upd = 0.5 * (P_upd + P_upd');  % 强制对称
[V, D] = eig(P_upd);
D = diag(max(diag(D), 1e-6));    % 防止非正定
P_upd = V * D * V';
end

---

三、C 语言版本（STM32 可用）

/* jpda_simple.h */
#ifndef JPDA_SIMPLE_H
#define JPDA_SIMPLE_H

#include <math.h>
#include <string.h>

#define STATE_DIM 4
#define MEAS_DIM 2
#define MAX_OBS 10

typedef struct {
    float x[STATE_DIM];      // 状态
    float P[STATE_DIM][STATE_DIM];  // 协方差
} TargetState;

typedef struct {
    float z[MEAS_DIM];        // 测量值
    float beta;               // 关联概率
} Measurement;

float mahalanobis_distance(const float *innov, const float S_inv[MEAS_DIM][MEAS_DIM]);
void jpda_update(TargetState *target, Measurement measurements[], int n_obs,
                 const float C[MEAS_DIM][STATE_DIM],
                 const float R[MEAS_DIM][MEAS_DIM],
                 float PD, float PG, float lambda, float gate_thresh);

#endif /* JPDA_SIMPLE_H */

/* jpda_simple.c */
#include "jpda_simple.h"

// 矩阵乘法: C = A * B
static void matrix_multiply(float *C, const float *A, const float *B,
                           int rows_a, int cols_a, int rows_b, int cols_b) {
    memset(C, 0, rows_a * cols_b * sizeof(float));
    for (int i = 0; i < rows_a; i++) {
        for (int j = 0; j < cols_b; j++) {
            for (int k = 0; k < cols_a; k++) {
                C[i*cols_b + j] += A[i*cols_a + k] * B[k*cols_b + j];
            }
        }
    }
}

// 计算马氏距离
float mahalanobis_distance(const float *innov, const float S_inv[MEAS_DIM][MEAS_DIM]) {
    float d2 = 0;
    for (int i = 0; i < MEAS_DIM; i++) {
        for (int j = 0; j < MEAS_DIM; j++) {
            d2 += innov[i] * S_inv[i][j] * innov[j];
        }
    }
    return d2;
}

// 简化 JPDA 更新
void jpda_update(TargetState *target, Measurement measurements[], int n_obs,
                 const float C[MEAS_DIM][STATE_DIM],
                 const float R[MEAS_DIM][MEAS_DIM],
                 float PD, float PG, float lambda, float gate_thresh) {
    
    // 1. 预测步骤（假设已在外部完成）
    
    // 2. 计算新息协方差 S = C*P*C' + R
    float CP[MEAS_DIM][STATE_DIM];
    float S[MEAS_DIM][MEAS_DIM];
    float S_inv[MEAS_DIM][MEAS_DIM];
    
    // CP = C * P
    matrix_multiply((float*)CP, (float*)C, (float*)target->P, 
                   MEAS_DIM, STATE_DIM, STATE_DIM, STATE_DIM);
    
    // S = CP * C' + R
    float CPCt[MEAS_DIM][MEAS_DIM];
    matrix_multiply((float*)CPCt, (float*)CP, (float*)C, 
                   MEAS_DIM, STATE_DIM, STATE_DIM, MEAS_DIM);
    
    for (int i = 0; i < MEAS_DIM; i++) {
        for (int j = 0; j < MEAS_DIM; j++) {
            S[i][j] = CPCt[i][j] + R[i][j];
        }
    }
    
    // 3. 计算 S 的行列式（简化：假设对角阵）
    float det_S = 1.0f;
    for (int i = 0; i < MEAS_DIM; i++) {
        det_S *= S[i][i];
    }
    
    // 4. 计算 S 的逆（简化：假设对角阵）
    for (int i = 0; i < MEAS_DIM; i++) {
        for (int j = 0; j < MEAS_DIM; j++) {
            S_inv[i][j] = (i == j) ? 1.0f / S[i][i] : 0.0f;
        }
    }
    
    // 5. 计算卡尔曼增益 K = P * C' * inv(S)
    float PCt[STATE_DIM][MEAS_DIM];
    matrix_multiply((float*)PCt, (float*)target->P, (float*)C,
                   STATE_DIM, STATE_DIM, MEAS_DIM, MEAS_DIM);
    
    float K[STATE_DIM][MEAS_DIM];
    matrix_multiply((float*)K, (float*)PCt, (float*)S_inv,
                   STATE_DIM, MEAS_DIM, MEAS_DIM, MEAS_DIM);
    
    // 6. 计算关联概率
    float beta[MAX_OBS] = {0};
    float beta_total = 0;
    
    for (int j = 0; j < n_obs; j++) {
        // 计算新息
        float innov[MEAS_DIM];
        for (int i = 0; i < MEAS_DIM; i++) {
            innov[i] = measurements[j].z[i];
            for (int k = 0; k < STATE_DIM; k++) {
                innov[i] -= C[i][k] * target->x[k];
            }
        }
        
        // 马氏距离
        float d2 = mahalanobis_distance(innov, S_inv);
        
        // 门检查
        if (d2 <= gate_thresh) {
            // 高斯似然
            beta[j] = expf(-0.5f * d2) / sqrtf((2.0f * 3.1415926535f) * det_S);
            beta_total += beta[j];
        }
    }
    
    // 7. 加入漏检和杂波
    float beta_0 = 1.0f - PD * PG;  // 漏检
    beta_total += beta_0 + lambda;
    
    // 8. 归一化概率
    for (int j = 0; j < n_obs; j++) {
        measurements[j].beta = beta[j] / beta_total;
    }
    beta_0 /= beta_total;
    
    // 9. 加权更新
    float weighted_innov[STATE_DIM] = {0};
    
    for (int j = 0; j < n_obs; j++) {
        if (measurements[j].beta > 1e-6) {
            // 计算新息
            float innov[MEAS_DIM];
            for (int i = 0; i < MEAS_DIM; i++) {
                innov[i] = measurements[j].z[i];
                for (int k = 0; k < STATE_DIM; k++) {
                    innov[i] -= C[i][k] * target->x[k];
                }
            }
            
            // K * innov
            float K_innov[STATE_DIM] = {0};
            for (int i = 0; i < STATE_DIM; i++) {
                for (int k = 0; k < MEAS_DIM; k++) {
                    K_innov[i] += K[i][k] * innov[k];
                }
            }
            
            // 加权累加
            for (int i = 0; i < STATE_DIM; i++) {
                weighted_innov[i] += measurements[j].beta * K_innov[i];
            }
        }
    }
    
    // 10. 更新状态
    for (int i = 0; i < STATE_DIM; i++) {
        target->x[i] += weighted_innov[i];
    }
}

参考代码 简化的JPDA算法 www.youwenfan.com/contentcnu/53316.html

四、参数调优指南

参数	建议范围	影响	调优建议
PD (检测概率)	0.7-0.99	漏检项权重	实际检测概率，保守估计
PG (门概率)	0.95-0.99	关联门大小	门越大，关联越多
λ (杂波密度)	0.01-1.0	杂波影响	单位面积杂波数，实际测量
门限 γ	9.21-13.8	关联门阈值	对应95%-99.9%置信度
Q (过程噪声)	0.001-0.1	模型不确定性	目标机动性大→增大
R (观测噪声)	0.01-1.0	测量精度	传感器噪声特性

---

五、嵌入式优化建议

1. 计算优化

// 使用定点运算（Q格式）
typedef int32_t q16_t;
#define Q16_SHIFT 16

q16_t q16_mul(q16_t a, q16_t b) {
    return (q16_t)(((int64_t)a * b) >> Q16_SHIFT);
}

// 近似指数函数（避免浮点）
q16_t exp_approx(q16_t x) {
    // 使用查表法或泰勒展开
    static const q16_t exp_table[] = { ... };
    return exp_table[x >> 8];
}

2. 内存优化

// 使用池分配器减少内存碎片
typedef struct {
    Measurement pool[MAX_TOTAL_OBS];
    uint8_t used[MAX_TOTAL_OBS];
} MeasurementPool;

3. 实时性保证

• 限制最大观测数 (e.g., ≤20)
• 使用提前终止：概率太小则跳过
• 并行计算：多目标可并行处理

---

六、性能对比

算法	计算复杂度	精度	适用场景
最近邻 NN	O(MN)	低	稀疏目标，无杂波
PDA	O(MN)	中	单目标，多杂波
简化 JPDA	O(MN)	中高	多目标，中等杂波
标准 JPDA	O(M!N!)	高	学术研究，离线处理
MHT	指数级	最高	高密度杂波，高精度

---