%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
    k=get_k_length(i);
    k11(j) = k(1,1);
    k12(j) = k(1,2);
    k13(j) = k(1,3);
    k14(j) = k(1,4);
    k15(j) = k(1,5);
    k16(j) = k(1,6);

    k21(j) = k(2,1);
    k22(j) = k(2,2);
    k23(j) = k(2,3);
    k24(j) = k(2,4);
    k25(j) = k(2,5);
    k26(j) = k(2,6);
    j=j+1;

    fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);

a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);

toc

% 打印格式化的C代码结构，可以直接复制到config.c中
fprintf('\n=========== 可直接复制的C代码 ===========\n');
fprintf('.lqr_gains = {\n');
fprintf('    .k11_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a11(1), a11(2), a11(3), a11(4));
fprintf('    .k12_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_theta\n', a12(1), a12(2), a12(3), a12(4));
fprintf('    .k13_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // x\n', a13(1), a13(2), a13(3), a13(4));
fprintf('    .k14_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_x\n', a14(1), a14(2), a14(3), a14(4));
fprintf('    .k15_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a15(1), a15(2), a15(3), a15(4));
fprintf('    .k16_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_phi\n', a16(1), a16(2), a16(3), a16(4));
fprintf('    .k21_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a21(1), a21(2), a21(3), a21(4));
fprintf('    .k22_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_theta\n', a22(1), a22(2), a22(3), a22(4));
fprintf('    .k23_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // x\n', a23(1), a23(2), a23(3), a23(4));
fprintf('    .k24_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_x\n', a24(1), a24(2), a24(3), a24(4));
fprintf('    .k25_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a25(1), a25(2), a25(3), a25(4));
fprintf('    .k26_coeff = { %.15ef, %.15ef, %.15ef, %.15ef },  // d_phi\n', a26(1), a26(2), a26(3), a26(4));
fprintf('},\n');
fprintf('========================================\n\n');

% 可选：显示拟合曲线图
x0=leg;              %步长为0.1
y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值

y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值

figure;
subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合');
subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合');
subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合');
subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合');
subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合');
subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合');

subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合');
subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合');
subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合');
subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合');
subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合');
subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合');

for i = 1:12
    subplot(3,4,i);
    grid on;                   %添加网格线
    set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5);  %将网格线变成虚线
    legend('原始数据','拟合曲线','Location','best');
end

toc