Subroutine to calculate the position of the center of mass from the position of the center of pressure
For stabilometric platforms
Author: Bernard Gagey bernard.gagey@gmail.com
Language:
Restrictions: This program
Input parameters
Output :
Explanation:
The body is assumed to behave like an inverted pendulum, and to satisfy to the differential equation:
P = M-k2.M''
All the solutions of this equation are given, from a first Mø solution by the formula
M=Mø +(aekt+be-kt)
First we search for the Mø solution that would be in O at the beep preceding and following the measuring. In order to do so, the differential equation is written in every point :
P(j)=M(j)-k2(M(j-1)+M(j+1)-2M(j))/dt2
Having assumed M(O) and M(N+1) = 0, that constitutes a system of N linear equations with N unknown values ; we have only to solve them or, in other words:
Vector P = matrix A * vector M
vector M = (matrix A)-1 *vector P
The first stage of the program consists in building the matrix
The second stage solves the linear system directly or by inverting the matrix
The third stage is meant to find good coefficients a and b. It is mathematically impossible to find them, but physically the points M(Ø) and M(N+1) must at least be on the platform. The formula used in the program was found empirically, but one knows that it is not, from afar, perfect. In a comment a 2nd formula easier to understand is put . In any case, after one or two seconds, the importance of this correction is insignificant.
% Starting
function[g]=SpG(posi,freqech,taille);
%Program looking for the curve G
% input parameters:
% posi the curve in complex form
% freqch sampling frequency
% taille Subject's size, must be corrected if the heigh of the center of gravity 0.55 is not used.
% retour
% g the last estimation of the curve g which satisfies posil=g -kg''
%
gravite=9.8066;
H_gravite=.55;%_____________ratio Center of Gravity Height/total height
Correction=1.2;% See 'Restrictions'
ak=H_gravite*Correction/gravite;
coeffaccel=ak*taille
kacc=(1/coeffaccel)^.5;
Nbm=length(posi);
% center so that the point (0,0) is not absurd
meanposi=mean(posi);
posi=posi-meanposi;
% Matrix coefficients calculation
co1=-freqech*freqech*coeffaccel;co2=-2*co1+1;
% Creating the maatrix with its right diagonal ma=co2*eye(Nbm,Nbm);
%r sub et super-diagonal
for k=2:1:Nbm-1;ma(k,k-1)=co1;ma(k,k+1)=co1;endfor
ma(1,2)=co1;ma(Nbm,Nbm-1)=co1;
% First method: Matrix inversion, quicker with Octave that works with matrices
na=inv(ma);
g=na*posi;
% Second method (as commentary) by resolution of linear system that ought be quicker
%g=ma\posi;
% looking for beginning and end
% first iteration to get the same first point and tprefirst point
correc=zeros(Nbm,1);for jj=1:1:Nbm;correc(jj)=e^(-kacc*jj/freqech);endfor;
deb=g(1)/(1-correc(1));
fin=g(Nbm)/(1-correc(1));
% Second iteration
mul1=76;mul2=75;
deb=mul1*g(1)-mul2*g(2)+deb*(mul1*correc(1)-mul2*correc(2));
fin=mul1*g(Nbm)-mul2*g(Nbm-1)+fin*(mul1*correc(1)-mul2*correc(2));
% It is also possible to consider the extrem points of Vg are those of Vp
%deb=(Vp(1)-Vg(1))/correc(1);fin=(Vp(Nbm)-Vg(Nbm))/correc(1);
%Effectuer l'addition de la correction et oter le centrage
g=meanposi+(g+deb*correc+flipud(fin*correc));
endfunction