Compute regressor matrix numerically

[ediget]

The regressor matrix formulation of the dynamics for system identification can be computed iteratively and numerically using the RNEA-algorithm.
The procedure is as described in the following pseudocode:

function get_regressor(q, dq, ddq):
    parameter_vector =  [m1, m1lc1, m1lc2, ..., I1zz, ..., mNlc1, mNlc2, ..., INzz]; % 10*N

    set all elements of parameter_vector to 0

    Y = zeros(N, 10 * N)

    for i = 1:length(parameter_vector)
        parameter_vector[i] = 1
        Y[:, i] = RNEA(q, dq, ddq, parameter_vector)
    end

    restore values of parameter_vector to original values
end