solveequalities.m

matlab波形优化算法经常要用到的matlab toolbox工具箱:yalmip

function [x_equ,H,A_equ,b_equ,factors] = solveequalities(F_struc,K,unitary)
%SOLVEEQUALITIES Internal function remove equality constraints

% Author Johan L鰂berg
% $Id: solveequalities.m,v 1.17 2007/05/24 14:44:20 joloef Exp $

% Extract the inequalities
A_equ = F_struc(1:K.f,2:end);
b_equ = -F_struc(1:K.f,1);

if nargin<3
    unitary = 1;
end

factors = [];

% remove = find((~any(A_equ,2)) & (b_equ == 0));
% if ~isempty(remove)
%     A_equ = A_equ';
%     A_equ(:,remove) = [];
%     A_equ = A_equ';
%     b_equ = b_equ';
%     b_equ(remove) = [];
%     b_equ = b_equ';
% end

if ~unitary

    % This code needs a clean up, and I need to take a course in numerical
    % analysis again.

    % Remove redundant constraints
    [L,U,P] = lu(A_equ);
    r = colspaces(U);
    if ~(length(r) == size(U,1))
        b_equ =  L\(P*b_equ);
        A_equ = U(r,:);
        b_equ = b_equ(r);
    end

    % Find a basis for the column space of A_equ
    [L,U,P] = lu(A_equ');
    %  U'*L'*P = A_equ
    %  U'*L'*Px = b_equ
    %  L'*Px = (U')\b_equ
    %  L'*z = (U')\b_equ, z = Px
    %  [FULLRANK other]z = b
    r = colspaces(L');
    AA = L';
    H1 = AA(:,r);
    H2 = AA(:,setdiff(1:size(AA,2),r));
    try
        %x_equ = A_equ\b_equ;
        x_equ = P'*linsolve(full(L'),linsolve(full(U'),full(b_equ),struct('LT',1==1)),struct('UT',1==1));
    catch
        x_equ = A_equ\b_equ;
    end
    % FIX : use L and U stupid!
    H = P'*[-H1\H2;eye(size(H2,2))];
else
    %Improve numerics by removing obviously redundant constraints. This
    %speeds up some cases too
    hash = 1+rand(1+size(A_equ,2),1);
    [i,j] = unique([A_equ b_equ]*hash);
    remove =  setdiff(1:size(A_equ,1),j);
    if ~isempty(remove)
        A_equ(remove,:) = [];
        b_equ(remove) = [];
    end
    % Use unitary basis
    try
        [Q,R,E] = qr(full(A_equ)');
    catch
        [Q,R,E] = qr(A_equ'); % Ouch, that big!
    end
    n = max(find(sum(abs(R),2)>1e-14*size(R,2)));

    Q1 = Q(:,1:n);
    R = R(1:n,:);
    x_equ = Q1*(R'\E'*b_equ);
    H = Q(:,n+1:end); % New basis
end

function  [indx]=colspaces(A)
indx = [];
for i = 1:size(A,2)
    s = max(find(A(:,i)));
    indx = [indx s];
end
indx = unique(indx);