This notebook describes the details of forming the coefficient matrix and ordinate vector from walking data for a linear least squares solution to the optimal gains from a simple full state feedback controller employed by the human while walking.
from sympy import init_printing, symbols, Matrix, Symbol, zeros, eye
init_printing(print_builtin=False)
n = 3 # number of time samples in each gait cycle
m = 2 # number of gait cycles
p = 2 # number of sensors
q = 2 # number of controls
First form the equation that generates the controls given the error in the sensors at a single time step during a single foot step.
$$ \begin{align} \mathbf{m}(t) & = & \mathbf{m}_0(\varphi) + \mathbf{K}(\varphi) \mathbf{s}_e(t) \\ & = & \mathbf{m}_0(\varphi) + \mathbf{K}(\varphi) [\mathbf{s}_0(\varphi) - \mathbf{s}(t)] \end{align} $$Now rearrange the equations such that we have one linear in both the gains, $\mathbf{K}(\varphi)$, and in $\mathbf{m}^*(\varphi)$:
$$ \begin{align} \mathbf{m}(t) & = & \mathbf{m}_0(\varphi) + \mathbf{K}(\varphi) \mathbf{s}_0(\varphi) - \mathbf{K}(\varphi) \mathbf{s}(t) \\ & = & \mathbf{m}^*(\varphi) - \mathbf{K}(\varphi) \mathbf{s}(t) \end{align} $$The "measured" controls, $\mathbf{m}(t)$:
m = Matrix(q, 1, lambda i, j: Symbol('m_q={}'.format(i)))
m
The unknown gains, $\mathbf{K}(\varphi)$:
K = Matrix(q, p, lambda i, j: Symbol('k_q={},p={}'.format(i, j)))
K
The unknown nominal controls, $\mathbf{m}^*(\varphi)$:
ms = Matrix(q, 1, lambda i, j: Symbol('m^*_q={}'.format(i)))
ms
The measured sensors, $\mathbf{s}(t)$:
s = Matrix(p, 1, lambda i, j: Symbol('s_p={}'.format(i)))
s
And the equation:
$$\mathbf{0} = \mathbf{m}^*(\varphi) - \mathbf{K}(\varphi) \mathbf{s}(t) - \mathbf{m}(t)$$zero = ms - K * s - m
zero
Now for a simple linear least squares the equations can be massage into this form:
$$\mathbf{Ax}=\mathbf{b}$$which, for large $m$, gives and over determined system of linear equations.
We define $\mathbf{x}$ as:
x = K.reshape(q * p, 1).col_join(ms)
x
To form the $\mathbf{A}$ matrix for a single time step in a single foot step the equation can be rearranged:
A = zeros(q, q * p)
for row in range(q):
A[row, row * p:(row + 1) * p] = -s.T
A = A.row_join(eye(q))
A
b = m
b
Now we can check to make sure that our new form gives the same answer as the previous form:
zero2 = A * x - b
zero2
assert zero == zero2
Now for $n$ time steps in a gait cycle we want to form the $\mathbf{A}$ matrix such that there are different gains for each time step.
gain_matrices = []
control_vectors = []
control_star_vectors = []
sensor_vectors = []
for time_step in range(n):
gain_matrices.append(Matrix(q, p, lambda i, j:
Symbol('k^{}_q={},p={}'.format(time_step, i, j))))
control_vectors.append(Matrix(q, 1, lambda i, j:
Symbol('m^{}_q={}'.format(time_step, i))))
control_star_vectors.append(Matrix(q, 1, lambda i, j:
Symbol('m^*{}_q={}'.format(time_step, i))))
sensor_vectors.append(Matrix(p, 1, lambda i, j:
Symbol('s^{}_p={}'.format(time_step, i))))
gain_matrices
control_vectors
control_star_vectors
sensor_vectors
Picking off a trio we should get our familiar equation back:
zero = - control_vectors[0] + control_star_vectors[0] - gain_matrices[0] * sensor_vectors[0]
zero
The larger $\mathbf{A}$ matrix for all of the time steps can be formed by stacking the $\mathbf{A}$ matrices from a single time step diagonally:
Am = zeros(n * q, n * (q + q * p))
for time_step in range(n):
An = zeros(q, q * p)
for row in range(q):
An[row, row * p:(row + 1) * p] = -sensor_vectors[time_step].T
An = An.row_join(eye(q))
num_rows, num_cols = An.shape
Am[time_step * num_rows:(time_step + 1) * num_rows, time_step * num_cols:(time_step + 1) * num_cols] = An
Am
The control vector and the gain vector are simple column stacks:
bm = control_vectors[0]
for control in control_vectors[1:]:
bm = bm.col_join(control)
bm
xm = gain_matrices[0].reshape(q * p, 1).col_join(control_star_vectors[0])
for gain, control_star in zip(gain_matrices[1:], control_star_vectors[1:]):
xm = xm.col_join(gain.reshape(q * p, 1)).col_join(control_star)
xm
Now the original equations can be regenerated with:
Am * xm - bm
We have $n(p q + q)$ unknowns and $nq$ equations so we need at least $1 + p$ cycles to solve for the unknowns.
n * (p*q + q)
18
1 + p
3
Am.shape, bm.shape
((6, 18), (6, 1))
b = bm
A = Am
for step in range(p):
b = b.col_join(bm)
A = A.col_join(Am)
A.shape, b.shape
((18, 18), (18, 1))
%load_ext version_information
%version_information sympy
Software | Version |
---|---|
Python | 2.7.9 64bit [GCC 4.4.7 20120313 (Red Hat 4.4.7-1)] |
IPython | 3.0.0 |
OS | Linux 3.13.0 49 generic x86_64 with debian jessie sid |
sympy | 0.7.6 |
Fri Apr 24 10:54:34 2015 PDT |