using matlab to simplify calculation

%%%simplify.m

syms nc_x nc_y nc_z diffPos_x diffPos_y diffPos_z vc_x vc_y vc_z ...
    nw_x nw_y nw_z real;

d_Rsd_T = [nc_x * diffPos_x + vc_x * nw_x ...
           nc_y * diffPos_x + vc_y * nw_x ...
           nc_z * diffPos_x + vc_z * nw_x ...
           nc_x * diffPos_y + vc_x * nw_y ...
           nc_y * diffPos_y + vc_y * nw_y ...
           nc_z * diffPos_y + vc_z * nw_y ...
           nc_x * diffPos_z + vc_x * nw_z ...
           nc_y * diffPos_z + vc_y * nw_z ...
           nc_z * diffPos_z + vc_z * nw_z ...
           nw_x ...
           nw_y ...
           nw_z ];

syms dqb_q0_x dqb_q0_y dqb_q0_z dqb_q0_w ...
     dqb_qe_x dqb_qe_y dqb_qe_z dqb_qe_w real;

d_T_BDQ = [          0           0  -2*dqb_q0_y   -2*dqb_q0_z           0          0          0          0;
             -dqb_q0_z    dqb_q0_y     dqb_q0_x     -dqb_q0_w           0          0          0          0;
              dqb_q0_y    dqb_q0_z     dqb_q0_w      dqb_q0_x           0          0          0          0;
              dqb_q0_z    dqb_q0_y     dqb_q0_x      dqb_q0_w           0          0          0          0;
                     0 -2*dqb_q0_x            0   -2*dqb_q0_z           0          0          0          0;
             -dqb_q0_x   -dqb_q0_w     dqb_q0_z      dqb_q0_y           0          0          0          0;
             -dqb_q0_y    dqb_q0_z    -dqb_q0_w      dqb_q0_x           0          0          0          0;
              dqb_q0_x    dqb_q0_w     dqb_q0_z      dqb_q0_y           0          0          0          0;
                     0 -2*dqb_q0_x  -2*dqb_q0_y             0           0          0          0          0;
              dqb_qe_x   -dqb_qe_w     dqb_qe_z     -dqb_qe_y   -dqb_q0_x   dqb_q0_w  -dqb_q0_z   dqb_q0_y;
              dqb_qe_y   -dqb_qe_z    -dqb_qe_w      dqb_qe_x   -dqb_q0_y   dqb_q0_z   dqb_q0_w  -dqb_q0_x;
              dqb_qe_z    dqb_qe_y    -dqb_qe_x     -dqb_qe_w   -dqb_q0_z  -dqb_q0_y   dqb_q0_x   dqb_q0_w;
            ];

syms dqn_q0_x dqn_q0_y dqn_q0_z dqn_q0_w dqn_qe_x dqn_qe_y dqn_qe_z dqn_qe_w nw_x_a0 nw_x_ae;       

d_BDQ_se3 = [ -dqn_q0_x * nw_x_a0, -dqn_q0_y * nw_x_a0, -dqn_q0_z * nw_x_a0,  0,  0,  0;
               dqn_q0_w * nw_x_a0,  dqn_q0_z * nw_x_a0, -dqn_q0_y * nw_x_a0,  0,  0,  0;
              -dqn_q0_z * nw_x_a0,  dqn_q0_w * nw_x_a0,  dqn_q0_x * nw_x_a0,  0,  0,  0;
               dqn_q0_y * nw_x_a0, -dqn_q0_x * nw_x_a0,  dqn_q0_w * nw_x_a0,  0,  0,  0;
              -dqn_q0_x * nw_x_ae - dqn_qe_x * nw_x_a0, -dqn_q0_y * nw_x_ae - dqn_qe_y * nw_x_a0,...
              -dqn_q0_z * nw_x_ae - dqn_qe_z * nw_x_a0, -dqn_q0_x * nw_x_a0,...
              -dqn_q0_y * nw_x_a0, -dqn_q0_z * nw_x_a0;
               dqn_q0_w * nw_x_ae + dqn_qe_w * nw_x_a0, dqn_q0_z * nw_x_ae + dqn_qe_z * nw_x_a0,...
              -dqn_q0_y * nw_x_ae - dqn_qe_y * nw_x_a0, dqn_q0_w * nw_x_a0,...
               dqn_q0_z * nw_x_a0, -dqn_q0_y * nw_x_a0;
              -dqn_q0_z * nw_x_ae - dqn_qe_z * nw_x_a0, dqn_q0_w * nw_x_ae + dqn_qe_w * nw_x_a0,...
               dqn_q0_x * nw_x_ae + dqn_qe_x * nw_x_a0,-dqn_q0_z * nw_x_a0,...                                                                                                                                                                                                                                                                                                                         .
               dqn_q0_w * nw_x_a0,  dqn_q0_x * nw_x_a0;
               dqn_q0_y * nw_x_ae + dqn_qe_y * nw_x_a0, -dqn_q0_x * nw_x_ae - dqn_qe_x *nw_x_a0,...
               dqn_q0_w * nw_x_ae + dqn_qe_w * nw_x_a0,  dqn_q0_y * nw_x_a0,...
              -dqn_q0_x * nw_x_a0,  dqn_q0_w * nw_x_a0

               ];



d_Rsd_se3 = d_Rsd_T * d_T_BDQ * d_BDQ_se3;

 %process_string.m
result = {'(dqn_q0_w*nw_x_ae + dqn_qe_w*nw_x_a0)*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y) + (dqn_q0_x*nw_x_ae + dqn_qe_x*nw_x_a0)*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z) + (dqn_q0_y*nw_x_ae + dqn_qe_y*nw_x_a0)*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) - (dqn_q0_z*nw_x_ae + dqn_qe_z*nw_x_a0)*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x) - dqn_q0_x*nw_x_a0*(dqb_qe_x*nw_x + dqb_qe_y*nw_y + dqb_qe_z*nw_z - dqb_q0_z*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_z*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_y*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_y*(diffPos_z*nc_x + nw_z*vc_x) - dqb_q0_x*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_x*(diffPos_z*nc_y + nw_z*vc_y)) + dqn_q0_y*nw_x_a0*(dqb_qe_x*nw_y - dqb_qe_w*nw_z - dqb_qe_y*nw_x - dqb_q0_w*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_w*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_z*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_x*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_z*(diffPos_y*nc_y + nw_y*vc_y) + dqb_q0_y*(diffPos_y*nc_z + nw_\\\ny*vc_z) + dqb_q0_y*(diffPos_z*nc_y + nw_z*vc_y)) - dqn_q0_z*nw_x_a0*(dqb_qe_z*nw_x - dqb_qe_x*nw_z - dqb_qe_w*nw_y - 2*dqb_q0_y*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_x*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_w*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_w*(diffPos_z*nc_x + nw_z*vc_x) + dqb_q0_z*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_z*(diffPos_z*nc_y + nw_z*vc_y) - 2*dqb_q0_y*(diffPos_z*nc_z + nw_z*vc_z)) + dqn_q0_w*nw_x_a0*(dqb_qe_y*nw_z - dqb_qe_w*nw_x - dqb_qe_z*nw_y + dqb_q0_y*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_y*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_x*(diffPos_y*nc_y + nw_y*vc_y) - dqb_q0_w*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_w*(diffPos_z*nc_y + nw_z*vc_y) + dqb_q0_z*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_z*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_x*(diffPos_z*nc_z + nw_z*vc_z))';
          '(dqn_q0_w*nw_x_ae + dqn_qe_w*nw_x_a0)*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x) - (dqn_q0_x*nw_x_ae + dqn_qe_x*nw_x_a0)*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) + (dqn_q0_y*nw_x_ae + dqn_qe_y*nw_x_a0)*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z) + (dqn_q0_z*nw_x_ae + dqn_qe_z*nw_x_a0)*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y) - dqn_q0_y*nw_x_a0*(dqb_qe_x*nw_x + dqb_qe_y*nw_y + dqb_qe_z*nw_z - dqb_q0_z*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_z*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_y*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_y*(diffPos_z*nc_x + nw_z*vc_x) - dqb_q0_x*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_x*(diffPos_z*nc_y + nw_z*vc_y)) - dqn_q0_x*nw_x_a0*(dqb_qe_x*nw_y - dqb_qe_w*nw_z - dqb_qe_y*nw_x - dqb_q0_w*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_w*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_z*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_x*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_z*(diffPos_y*nc_y + nw_y*vc_y) + dqb_q0_y*(diffPos_y*nc_z + nw_\\\ny*vc_z) + dqb_q0_y*(diffPos_z*nc_y + nw_z*vc_y)) + dqn_q0_w*nw_x_a0*(dqb_qe_z*nw_x - dqb_qe_x*nw_z - dqb_qe_w*nw_y - 2*dqb_q0_y*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_x*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_w*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_w*(diffPos_z*nc_x + nw_z*vc_x) + dqb_q0_z*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_z*(diffPos_z*nc_y + nw_z*vc_y) - 2*dqb_q0_y*(diffPos_z*nc_z + nw_z*vc_z)) + dqn_q0_z*nw_x_a0*(dqb_qe_y*nw_z - dqb_qe_w*nw_x - dqb_qe_z*nw_y + dqb_q0_y*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_y*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_x*(diffPos_y*nc_y + nw_y*vc_y) - dqb_q0_w*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_w*(diffPos_z*nc_y + nw_z*vc_y) + dqb_q0_z*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_z*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_x*(diffPos_z*nc_z + nw_z*vc_z))';
          '(dqn_q0_w*nw_x_ae + dqn_qe_w*nw_x_a0)*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) + (dqn_q0_x*nw_x_ae + dqn_qe_x*nw_x_a0)*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x) - (dqn_q0_y*nw_x_ae + dqn_qe_y*nw_x_a0)*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y) + (dqn_q0_z*nw_x_ae + dqn_qe_z*nw_x_a0)*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z) - dqn_q0_z*nw_x_a0*(dqb_qe_x*nw_x + dqb_qe_y*nw_y + dqb_qe_z*nw_z - dqb_q0_z*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_z*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_y*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_y*(diffPos_z*nc_x + nw_z*vc_x) - dqb_q0_x*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_x*(diffPos_z*nc_y + nw_z*vc_y)) + dqn_q0_w*nw_x_a0*(dqb_qe_x*nw_y - dqb_qe_w*nw_z - dqb_qe_y*nw_x - dqb_q0_w*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_w*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_z*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_x*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_z*(diffPos_y*nc_y + nw_y*vc_y) + dqb_q0_y*(diffPos_y*nc_z + nw_\\\ny*vc_z) + dqb_q0_y*(diffPos_z*nc_y + nw_z*vc_y)) + dqn_q0_x*nw_x_a0*(dqb_qe_z*nw_x - dqb_qe_x*nw_z - dqb_qe_w*nw_y - 2*dqb_q0_y*(diffPos_x*nc_x + nw_x*vc_x) + dqb_q0_x*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_x*(diffPos_y*nc_x + nw_y*vc_x) + dqb_q0_w*(diffPos_x*nc_z + nw_x*vc_z) - dqb_q0_w*(diffPos_z*nc_x + nw_z*vc_x) + dqb_q0_z*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_z*(diffPos_z*nc_y + nw_z*vc_y) - 2*dqb_q0_y*(diffPos_z*nc_z + nw_z*vc_z)) - dqn_q0_y*nw_x_a0*(dqb_qe_y*nw_z - dqb_qe_w*nw_x - dqb_qe_z*nw_y + dqb_q0_y*(diffPos_x*nc_y + nw_x*vc_y) + dqb_q0_y*(diffPos_y*nc_x + nw_y*vc_x) - 2*dqb_q0_x*(diffPos_y*nc_y + nw_y*vc_y) - dqb_q0_w*(diffPos_y*nc_z + nw_y*vc_z) + dqb_q0_w*(diffPos_z*nc_y + nw_z*vc_y) + dqb_q0_z*(diffPos_x*nc_z + nw_x*vc_z) + dqb_q0_z*(diffPos_z*nc_x + nw_z*vc_x) - 2*dqb_q0_x*(diffPos_z*nc_z + nw_z*vc_z))';
          'dqn_q0_w*nw_x_a0*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y) + dqn_q0_y*nw_x_a0*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) + dqn_q0_x*nw_x_a0*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z) - dqn_q0_z*nw_x_a0*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x)';
          'dqn_q0_w*nw_x_a0*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x) - dqn_q0_x*nw_x_a0*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) + dqn_q0_y*nw_x_a0*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z) + dqn_q0_z*nw_x_a0*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y)';
          'dqn_q0_w*nw_x_a0*(dqb_q0_w*nw_z - dqb_q0_x*nw_y + dqb_q0_y*nw_x) + dqn_q0_x*nw_x_a0*(dqb_q0_w*nw_y + dqb_q0_x*nw_z - dqb_q0_z*nw_x) - dqn_q0_y*nw_x_a0*(dqb_q0_w*nw_x - dqb_q0_y*nw_z + dqb_q0_z*nw_y) + dqn_q0_z*nw_x_a0*(dqb_q0_x*nw_x + dqb_q0_y*nw_y + dqb_q0_z*nw_z)';
    };


result0 = cellfun(@strrep, result, repmat({'\\\n'},size(result)), repmat({''},size(result)), 'UniformOutput', 0); 
result1 = cellfun(@strrep, result0, repmat({'_x'},size(result0)), repmat({'.x'},size(result0)), 'UniformOutput', 0); 
result2 = cellfun(@strrep, result1, repmat({'_y'},size(result1)), repmat({'.y'},size(result1)), 'UniformOutput', 0);
result3 = cellfun(@strrep, result2, repmat({'_z'},size(result2)), repmat({'.z'},size(result2)), 'UniformOutput', 0);
result4 = cellfun(@strrep, result3, repmat({'_w'},size(result3)), repmat({'.w'},size(result3)), 'UniformOutput', 0);
result5 = cellfun(@strrep, result4, repmat({'nw.x_a0'},size(result4)), repmat({'nw_x_a0'},size(result4)), 'UniformOutput', 0);
result6 = cellfun(@strrep, result5, repmat({'nw.x_ae'},size(result5)), repmat({'nw_x_ae'},size(result5)), 'UniformOutput', 0);


result7 = char(result6);
arrayfun(@(x) result7(x,:), 1:6,'UniformOutput', 0 );