function [F] = poleplace(A, B, poles);
% poleplace.m
% 極配置により状態フィードバックゲインを求める
% 諸般の事情により，matlab 以外の toolbox が使えない(涙)
%
% 入力:
%   A, B : 状態方程式 dot x = A x + B u の係数行列
%   poles : 閉ループ系の固有値が入ったベクトル
% 出力:
%   F : (A - B F) の固有値を poles に一致させる状態フィードバックゲイン

n = size(A,1);
p0_tmp = poly(poles); % 目的とする特性多項式の係数
p_tmp = poly(A); %
p0 = [];
p = [];
for k=2:n+1
  p = [p_tmp(k), p];
  p0 = [p0_tmp(k), p0];
end

F1 = p - p0; % 可制御正準形に対する状態フィードバックゲイン

% もとの系 (A, B) に対する状態フィードバックゲイン F は，
% A1 = T\A*T, B1 = T\B を満足する行列 T を用いて，
% F = F1/T として求まる．
% ただし，(A1, B1) は可制御正準形

% 可制御正準形の計算
A1 = [zeros(n-1,1), eye(n-1); -p];
B1 = [zeros(n-1,1); 1];

% T を求める
T = [];
tmp = B;
for k=n:-1:1
  T = [tmp, T];
  tmp = A*tmp + p(k)*B;
end

F = F1/T;
