function [tauStar,cStar,u,x] = optTraj(x0,x1,h,B,R)
%OPTTRAJ 此处显示有关此函数的摘要
% 输 出：
% x     最优轨迹，？X3矩阵
% u     控制量，？X3矩阵
% cStar 最优轨迹x的代价，标量
% t     时间，列向量
% 输 入：
% x0初始位置列向量，x1末端位置列向量，h时间步长标量，B是6X3输人矩阵，R是权3X3权重矩阵
% 使用函数tauStarSolve.m、rungeKutta4.m和dmdt.m
% 作   者：鲁 鹏（北京理工大学宇航学院）
% 版   本：1.0
% version 1.0 更新日志：

% x0 = [0 0 0 0 0 0]'; % 初始位置列向量
% x1 = [10 10 10 0 0 0]'; % 末端位置列向量
% h  = 0.01; % 时间步长，四阶龙格库塔法等使用
[tauStar, cStar, xBarStar, GStar] = tauStarSolve(x0, x1, h);
dTauStar = GStar\(x1 - xBarStar);
hTauStar = [x1; dTauStar]; % 微分方程的终端状态

% 应为是从tauStar朝0积分，所以步长要是‘-h’
[~, m] = rungeKutta4(@dmdt, tauStar, 0 , hTauStar, -h); 

% figure
% plot3(h(:, 1), h(:, 2), h(:, 3))
u = zeros(size(m,1), 3);
K = B / R';
for i = 1 : size(m,1)
    u(i, :) = m(i,7:12)*K;
end
% 由于是反向求微分方程，所以要将输出结果上下颠倒
x = flipud(m(:,1:6));
u = flipud(u);
end