rlocus_cross.m

clear; close all; clc

%% 1. 系统定义
%z = -4; p = [0; -2; -1+3j; -1-3j];
%sys = zpk(z, p, 1);

%z = -2; p = [0; -1; -3+3j; -3-3j];
%sys = zpk(z, p, 1);

%z = []; p = [0; -3; -1+1j; -1-1j];
%sys = zpk(z, p, 1);

num = [1, 2, 4];
den = conv(conv([1, 4, 0], [1, 6]), [1, 1.4, 1]);
sys = tf(num, den);

[num_vec, den_vec] = tfdata(sys, 'v');  % 提取多项式系数（用于验证）

%% 2. 核心参数设置
tol_real = 1e-6;           % 实部阈值（判定跨轴）
tol_imag = 1e-6;           % 虚部阈值（过滤实轴点）
min_k = 0;                 % 最小有效增益（根轨迹K≥0）
tol_bisection = 1e-10;     % 二分法精度（实部绝对值≤此值）
max_iter_bisection = 100;  % 二分法最大迭代次数
tol_residual = 1e-6;       % 特征方程残差阈值（验证解有效性）

%% 3. 获取根轨迹基础数据
[r, k_vals] = rlocus(sys);  % r: 分支数×增益点数
branch_num = size(r, 1);    % 根轨迹分支数
k_num = length(k_vals);     % 增益序列长度

%% 4. 第一步：检测所有有效跨轴的相邻点，收集待二分的区间
valid_cross_intervals = []; % 存储[分支号, k1, k2, ref_real, ref_imag]
cross_count = 0;            % 有效跨轴相邻点对数

% 遍历每个根轨迹分支
for branch_idx = 1:branch_num
    real_vals = real(r(branch_idx, :));  % 该分支实部序列
    imag_vals = imag(r(branch_idx, :));  % 该分支虚部序列
    
    % 遍历相邻增益点
    for k_idx = 1:k_num-1
        % 提取基础数据
        k1 = k_vals(k_idx);       % 前一增益
        k2 = k_vals(k_idx+1);   % 后一增益
        r1 = real_vals(k_idx);    % 点1实部
        i1 = imag_vals(k_idx);    % 点1虚部
        r2 = real_vals(k_idx+1);  % 点2实部
        i2 = imag_vals(k_idx+1);  % 点2虚部
        
        % --------------- 过滤无效点 ---------------
        if k1 < min_k && k2 < min_k; continue; end               % 负增益
        if isinf(r1) || isinf(r2) || isnan(r1) || isnan(r2); continue; end % 数值异常
        if abs(i1) < tol_imag && abs(i2) < tol_imag; continue; end % 实轴点
        
        % --------------- 判定实部位置 ---------------
        is_cross = (r1 < -tol_real && r2 > tol_real) || (r1 > tol_real && r2 < -tol_real);
        if is_cross
            cross_count = cross_count + 1;
            valid_cross_intervals = [valid_cross_intervals;
                branch_idx, k1, k2, r1, i1]; % 存参考极点（第一个点）
        end
    end
end

%% 5. 第二步：对每个有效区间，二分法搜寻虚轴交点
valid_intersections = []; % 存储[K, w, 残差]

% 打印跨轴区间核心信息
if cross_count > 0
    fprintf('共检测到 %d 个有效跨轴区间，开始二分法搜寻虚轴交点...\n', cross_count);
    fprintf('--------------------------------------------------------\n');
    fprintf('区间序号 | 分支编号 | 初始增益区间 [k_low, k_high]\n');
    fprintf('--------------------------------------------------------\n');
    for i = 1:size(valid_cross_intervals, 1)
        fprintf('%02d       | %02d      | [%.4f, %.4f]\n', ...
            i, valid_cross_intervals(i, 1), valid_cross_intervals(i, 2), valid_cross_intervals(i, 3));
    end
    fprintf('--------------------------------------------------------\n\n');
    
    % 遍历每个有效跨轴区间
    for i = 1:size(valid_cross_intervals, 1)
        branch_idx = valid_cross_intervals(i, 1);
        k_low = valid_cross_intervals(i, 2);
        k_high = valid_cross_intervals(i, 3);
        ref_real = valid_cross_intervals(i, 4);
        ref_imag = valid_cross_intervals(i, 5);
        ref_pole = ref_real + 1i*ref_imag;
        
        % 二分法初始化
        iter = 0;
        k_best = (k_low + k_high) / 2;
        w_best = ref_imag;
        real_best = inf;
        pole_ref = ref_pole;
        interval_valid = true;
        
        % 二分法迭代
        while iter < max_iter_bisection && abs(real_best) > tol_bisection && interval_valid
            iter = iter + 1;
            
            % 验证区间两端实部符号
            poles_low = rlocus(sys, k_low);
            [min_dist_low, match_idx_low] = min(abs(poles_low - pole_ref));
            real_low = real(poles_low(match_idx_low));
            poles_high = rlocus(sys, k_high);
            [min_dist_high, match_idx_high] = min(abs(poles_high - pole_ref));
            real_high = real(poles_high(match_idx_high));
            
            sign_low = sign(real_low);
            sign_high = sign(real_high);
            if sign_low == sign_high && abs(real_low) > tol_bisection && abs(real_high) > tol_bisection
                interval_valid = false;
                break;
            end
            
            % 计算中间点
            k_mid = (k_low + k_high) / 2;
            poles_mid = rlocus(sys, k_mid);
            [min_dist, match_idx] = min(abs(poles_mid - pole_ref));
            pole_mid = poles_mid(match_idx);
            real_mid = real(pole_mid);
            imag_mid = imag(pole_mid);
            sign_mid = sign(real_mid);
            
            % 更新最优解
            if abs(real_mid) < abs(real_best)
                real_best = real_mid;
                k_best = k_mid;
                w_best = abs(imag_mid);
            end
            
            % 收缩区间
            if abs(real_mid) <= tol_bisection
                break;
            elseif sign_mid == sign_low && sign_low ~= 0
                k_low = k_mid;
            elseif sign_mid == sign_high && sign_high ~= 0
                k_high = k_mid;
            else
                break;
            end
            
            pole_ref = pole_mid;
        end
        
        % 验证解的有效性
        if interval_valid
            s_jw = 1i * w_best;
            residual = abs(polyval(den_vec, s_jw) + k_best * polyval(num_vec, s_jw));
            if residual < tol_residual && k_best > 0 && w_best > 1e-6
                valid_intersections = [valid_intersections; k_best, w_best, residual];
            end
        end
    end
    
    % 结果去重
    if ~isempty(valid_intersections)
        [~, unique_idx] = unique(round(valid_intersections(:,1:2)*1e8)/1e8, 'rows');
        valid_intersections = valid_intersections(unique_idx, :);
    end
else
    fprintf('未检测到任何有效跨轴区间\n');
    return;
end

%% 6. 打印最终结果（K保留4位小数）
if ~isempty(valid_intersections)
    fprintf('共找到 %d 个有效虚轴交点（去重后）：\n', size(valid_intersections, 1));
    fprintf('--------------------------------------------------------\n');
    fprintf('序号 | 临界增益K | 虚轴频率w | 特征方程残差\n');
    fprintf('--------------------------------------------------------\n');
    for i = 1:size(valid_intersections,1)
        K = valid_intersections(i, 1);
        w = valid_intersections(i, 2);
        res = valid_intersections(i, 3);
        fprintf('%02d   | %.4f      | %.4f    | %.e\n', i, K, w, res);
    end
else
    fprintf('未找到有效的根轨迹与 虚轴交点（除原点外）\n');
end
fprintf('========================================================\n');

%% 7. 绘制根轨迹与交点
figure('Color', 'w');
hold on; grid on; axis equal;
title('根轨迹与虚轴交点', 'FontSize', 12);
xlabel('实轴', 'FontSize', 10); ylabel('虚轴', 'FontSize', 10);

% 绘制根轨迹
for branch_idx = 1:branch_num
    plot(real(r(branch_idx,:)), imag(r(branch_idx,:)), 'b-', 'LineWidth', 1.2);
end

% 标记极点/零点
p = pole(sys); z = zero(sys);
plot(real(p), imag(p), 'rx', 'MarkerSize', 8, 'LineWidth', 1.5);
if ~isempty(z)
    plot(real(z), imag(z), 'ro', 'MarkerSize', 8, 'LineWidth', 1.5);
end

% 标记虚轴交点
if ~isempty(valid_intersections)
    for i = 1:size(valid_intersections, 1)
        K = valid_intersections(i, 1);
        w = valid_intersections(i, 2);
        plot(0, w, 'r.', 'MarkerSize', 12);
        plot(0, -w, 'r.', 'MarkerSize', 12);
        text(0.3, w+0.2, sprintf('K=%.2f', K), 'FontSize', 9);
        text(0.3, -w-0.3, sprintf('K=%.2f', K), 'FontSize', 9);
    end
end

xlim([-8, 4]); ylim([-5, 5]);
box on; hold off;