I can't get my Recursive Least Square algorithm work - What have I miss?

I'm want to do a recursive least square algorithm but I can't get it to work. If you don't know what recursive least square algorithm is. Well, it just ordinary least square, but it's an algorithm which works as online estimator for estimating a mathematical model, every iteration.

So let's say what we have a process $H(s)$ (transfer function symbol) and we are giving the process a constant signal $u(t) = 1$ for $t = [0, 15]$ seconds.

We don't know the process $H(s)$, but we know the input $u(t)$ and the output $y(t)$. But we know the that the process contains acceleration, and I will give us the information that $H(s)$ is a second order process.

We give the process $H(s)$ the $u(t)$ signal and measure the output $y(t)$. The template of the second order process can be described as:

$$G(s) = \frac{K}{s^2 + as + b}$$

But notice that we have collect the input and output signal with a sampling rate per $0.1$ time units. So we need to have this as a discrete process.

$$G_d(z) = \frac{a_0z + 1}{b_0z^2 + b_1z + 1}$$

Remember that $z^2 = (k+2)$ and $z = (k+1)$.

We can expand $G_d(z)$ into a discrete ODE with difference equations:

$$b_0y(k+2) + b_1y(k+1) + y(k) = a_0u(k+1) + a_1u(k)$$

Then we can change this into:

$$y(k) = -b_0y(k+2) - b_1y(k+1) + a_0u(k+1) + a_1u(k)$$

We know that the scalar of input $u(k)$ and output $y(k)$ is 1 because we have measure them both. But when it comes to estimation, we need to estimate the $a_1$ scalar too.

What we don't know is: $b_0, b_1, a_0, a_1$ and that is why we are using Least Square.

Image that we have a lot of mesaruments.

$$y(0) = -b_0y(2) - b_1y(1) + a_0u(1) + a_1u(0)$$ $$y(1) = -b_0y(3) - b_1y(2) + a_0u(2) + a_1u(1)$$ $$y(2) = -b_0y(4) - b_1y(3) + a_0u(3) + a_1u(2)$$ $$\vdots$$ $$y(k) = -b_0y(k+2) - b_1y(k+1) + a_0u(k+1) + a_1u(k)$$

Then we can express those like:

$$b = Ax$$

Where $A$ is: $$y(0) = -y(2) - (1) + u(1) + u(0)$$ $$y(1) = -(3) - (2) + u(2) + u(1)$$ $$y(2) = -y(4) - y(3) + u(3) + u(2)$$ $$\vdots$$ $$y(k) = -(k+2) - y(k+1) + u(k+1) + u(k)$$

And $x$ as:

$$[b_0, b_1, a_0, a_1]^T$$

and $b$ as: $$[y(k),y(k+1), y(k+2), \dots, y(k)]^T$$

We know $b$ and $A$, and we need to find $x$. A simple answer is:

$$x = A^{-1}b$$

That's how we can do least square! So now back the Recursive Least Square algorithm.

We first need to identify $\phi(k)$ as

$$\phi^T(k) = -(k+2) - y(k+1) + u(k+1) + u(k)$$

And our estimated scalars $\hat{\theta}$ as:

$$\hat{\theta}^T = [b_0, b_1, a_0, a_1]$$

Now we setup the Recursive Least Square algorithm as:

$$P(k) = P(k-1) - P(k-1)\phi(k)\phi^T(k)P(k-1)[1+\phi^T(k)P(k-1)\phi(k)]^{-1}$$

$$\hat{\theta} = \hat{\theta}(k-1) + P(k-1)\phi(k)[y(k) - \phi^T(k)\hat{\theta}(k-1)]$$

Where $P(k-1)$ and $\hat{\theta}(k-1)$ is just the past value. $P(k)$ is also called "updater".

Before we start the algoritm, we need to set some initial conditions for $P$ and $\hat{\theta}$.

For $P$ we set the value as:

$$P = cI$$

Where $c$ is a large number, say 1000, and $I$ is the identity matrix.

For $\hat{\theta}$ we set those to:

$$\hat{\theta} = 0$$

You can read more here about Recursive Least Square

Anyway! I have made an algorithm in MATLAB/Octave for estimating $\hat{\theta}$.

% Time and input
t = 0:0.1:15;
u = linspace(1,1, length(t));

% Continues time process
G = tf(, [1 2 3]);

% Turn it to discrete with sampling time 0.1
Gd = c2d(G, 0.1) % 0.1 seconds sample time

% Simulate the process.
[y, t] = lsim(Gd, u, t);

% Initial theta
Theta = [0;0;0;0];

% Initial P
c = 1000; % A large number
I = eye(length(Theta));
P = c*I;

% Estimation
n = length(u) -2 ; % 2 because (k_max + 2) does not exist.
for k = 1:n
phi = [-y(k+2); -y(k+1); u(k+1); u(k)];       % Update
P = P - P*phi*phi'*P/(1 + phi'*P*phi);   % Update
Theta = Theta + P*phi*(y(k) - phi'*Theta);
end

% Theta
Theta

And Theta becomes:

Theta =

-0.477237
-0.578289
-0.010080
-0.010080

That means:

$$y(k) = -b_0y(k+2) - b_1y(k+1) + a_0u(k+1) + a_1u(k)$$

$b_0 = -0.477237$, $b_1 = -0.578289$, $a_0 = -0.010080$, $a_1 = -0.010080$,

$$-0.477237(k+2) - 0.578289y(k+1) + y(k) = -0.010080u(k+1) - 0.010080u(k)$$

Which will be:

$$G_d(z) = \frac{-0.010080z - 0.010080}{-0.477237z^2 - 0.578289z + 1}$$

And this will give a very unstable process, because the poles of $G_d(z)$ are:$-2.17510, 0.9633$6

But writing $G_d(z)$ as:

$$G_d(z) = \frac{0.010080z + 0.010080}{0.477237z^2 + 0.578289z + 1}$$

Results stable poles $$-0.6059 + 1.3147i\\ -0.6059 - 1.3147i$$

But the simulation doesn't look great at all. Question:

This sound not right to me. Have I done something wrong in the estimation?

Edit:

A more theoretical explonation for $P(k)$ is that $P(k)$ can be expressed as:

$$P(k) = [\Phi^T(k)\Phi(k)]^{-1}$$

Because

$$b = Ax$$

Where $b$ is equivalent with $y(k)$ and $A$ is equivalent with $\Phi(k)$

What will gives us:

$$\hat{\theta} = [\Phi^T(k)\Phi]^{-1}\Phi^T(k)y(k) = \frac{y(k)}{\Phi(k)}$$

And every body knows the formula for least square with matrices?

$$x = (A^TA)^{-1}A^Tb = A^{-1}b$$

Edit2:

Here is a modification for RLS, which gives a better step answer of the transfer function.

% Time and input
t = 0:0.1:15;
u = linspace(1,1, length(t));

% Continues time process
G = tf(, [1 2 3]);

% Turn it to discrete with sampling time 0.1
Gd = c2d(G, 0.1) % 0.1 seconds sample time

% Simulate the process.
[y, t] = lsim(Gd, u, t);

% Initial theta
Theta = [0;0;0;0;0];

% Initial P
c = 1000; % A large number
I = eye(length(Theta));
P = c*I;

% Estimation
n = length(u);
for k = 1:n
if k <= 3
phi = 0;
else
phi = [-y(k-1); -y(k-2); -y(k-3); u(k-1); u(k-2)];       % Update
P = P - P*phi*phi'*P/(1 + phi'*P*phi);   % Update
Theta = Theta + P*phi*(y(k) - phi'*Theta);
end
end

% Theta
Theta

Gd2 = tf([Theta(4) Theta(5)],[1 Theta(1) Theta(2) Theta(3)]);
Gd2.sampleTime = 0.1;

figure(2)
step(Gd2, 15); • Your poles are not stable. Notice that you are looking at a discrete transfer function. Here you have to check whether the poles lie within the unit circle. Only in the continous case, negative real part of the poles implies stability. Here you have $\sqrt{0.6059^2 + 1.3147^2} \approx 1.3164 > 1$. Therefore your poles do not lie within the unit circle and your transfer function is unstable, as indicated by simulation. – SampleTime Feb 11 '18 at 13:40
• @SampleTime OK. I forgot that! But still, I cannot estimate a good model for some reason. As I see, I have done everything right, not the results shows that I haven't. – Daniel Mårtensson Feb 11 '18 at 13:47

I think you misinterpreted the discrete transfer function. Namely often the coefficient in front of the highest power of $z$ in the denominator is set to one, such that in your case it can be written as

$$G_d(z) = \frac{a_1\,z + a_0}{z^2 + b_1\,z + b_0}$$

which can be rewritten to the following difference equation

$$y[k+2] = a_1\,u[k+1] + a_0\,u[k] - b_1\,y[k+1] - b_0\,y[k].$$

It can also be noted that your difference equation is none-causal. Namely one needs to know the future states of the system in order to calculate the current state.

So if you want to identify the coefficients $\theta = \begin{bmatrix}b_0 & b_1 & a_0 & a_1\end{bmatrix}^\top$ using RLS such that

$$y(t) = \varphi^\top \!(t)\,\theta$$

then using the difference equation one has to define $\varphi(t)$ as

$$\varphi(t) = \begin{bmatrix} -y(t-2) & -y(t-1) & u(t-1) & u(t-2) \end{bmatrix}^\top.$$

It also has to be noted that RLS assumes persistence of excitation, which I believe is not the case for $u(t)$ equal to a step. If you do use a step input then that will probably result in a misidentification of $a_0$ and $a_1$. This however should not affect the poles and thus stability of the identified system.

• If you look at this code I have estimated at this way: $$\varphi(t) = \begin{bmatrix} -y(t-1) & -y(t-2) & u(t-1) & u(t-2) \end{bmatrix}^\top.$$ github.com/DanielMartensson/mataveid/blob/master/sourcecode/… – Daniel Mårtensson Feb 12 '18 at 17:01
• Try the code by the way. – Daniel Mårtensson Feb 12 '18 at 17:38
• @DanielMårtensson That code uses normal least squares and not RLS, so it is not entirely related to the scope of your question. But yes, I think that should work as well, only I am missing the delay in $u$ data when fitting. – Kwin van der Veen Feb 13 '18 at 0:08
• Hi. I succeeded to do a RLS algorithm. You can see that in "Edit2" in my question. I don't know why I RLS estimated a transfer function with critical damping? About the delay. The delay is constructed by c2dt(sys, delay) in Matavecontrol. – Daniel Mårtensson Feb 13 '18 at 12:08
• @DanielMårtensson The difference in transfer function is probably due to the fact that $u$ does not satisfy persistence of excitation. And for the delay, your fitted model will not give the same response, since you only use the delay when defining the transfer function at the end. If you would offset the u vector when constructing A2 then it should yield the same response I think. – Kwin van der Veen Feb 13 '18 at 13:29