Mechatronics Exercises

# Temperature box state estimation and control

Structure of the control algorithm

### The temperature control of the constant temperature box is divided to three parts:

1. Get/Update the physical model of the system to account for changes in the operating conditions of the box.
1. Lowering fan speed for example causes increase in temperature sensor time constant
2. Use state estimator to estimate the state(or states) of the system.
1. For this model these states are: heater temperature, air temperature inside box, sample temperature and ambient temperature outside box.
3. Use optimal control to calculate best possible control for getting from the estimated state to the desired state.
1. This uses a costfunction that taxes state error and control values.

All of the forementioned parts are implemented in their own functions. The advantage of this is that the physical model parameters are all in one place. If the controlled system changes you theoretically only need to modify this single function.

Because the state estimator is optimal you should never need to touch the function unless you need to fix a bug or reduce the memory footprint etc.

The control algorithm is also optimal and you shouldn't need to touch it either. You can however easily replace it with a control algorithm of your choosing and use the state estimate provided by the estimator.

However touching all the functions is required if you desire to modify the number of states the system has. Modifying the number of states affects the dimensions of the matrix calculation used in the other function.

Because the matrix library used has to be provided with the matrix dimensions of each input matrix. An additional library for determining the demensions or using macro definitions for the dimensions might be a good idea.

# Physical model of the system and the model parameter function

The heat transfer inside the system is modelled by using the Newtons law of cooling: (http://en.wikipedia.org/wiki/Convective_heat_transfer#Newton.27s_law_of_cooling)

This means that the heat transfer by radiation inside the box is considered insignificant.

We also assume that all heat transfer inside the box happens through the air. This means that the temperature of the heaters for example can not affect the temperature of the sample without going through the air.

The model has also some other pieces of physics involved. For example the temperature sensor is expected to follow the real temperature like a negative exponent exponential function closing in to its asymptote with a given time constant.

#### The used function is defined as follows:

`void temperatureBoxModel(float y_t, float t, float u_t, float fanPower , float* a_0, float* a_1,float* a_2, float* b_1, float* A_0, float* A_1, float* B_2)`

Function input are:

`float y_t = "previous measurement"`
`float t = "current time"`
`float u_t = "last control value"`
`float fanPower = "speed of the fan"`

Function outputs are the model parameters explained below:

`float* a_0, float* a_1,float* a_2, float* b_1, float* A_0, float* A_1, float* B_2`

#### The physical model is represented as a stochastic differrential equation of form:

`dz = (a_0(t,y) + a_1(t,y)z(t))dt + b_1(t,y)dW_1 + b_2(t,y)dW_2`
`dy = (A_0(t,y) + A_1(t,y)z(t))dt + B_1(t,y)dW_1 + B_2(t,y)dW_2`

This applies for continuous systems. To make it usable inside a computer we discretice this to receive the following form:

`z_th = z_t +(a_0(t,y) + a_1(t,y)*z_t)*h + b_1(t,y)*sqrt(h)*epsilon_1(t+h) + b_2(t,y)*sqrt(h)*epsilon_2(t+h)`
`y_th = y_t +(A_0(t,y) + A_1(t,y)*z_t)*h + B_1(t,y)*sqrt(h)*epsilon_1(t+h) + B_2(t.y)*sqrt(h)*epsilon_2(t+h)`

Here the first line tells us how the new state depends on the previous state. The second line model of the new measurement depends on the last measurement and the system state.

In this equation the parameters correspond to the following:

`z_t = "the previous state of the system"`
`z_th= "the new state of the system"`
`y_t = "the old measurement"`
`y_th= "the new measurement"`
`a_0 = "model dynamics that dont depend on the previous state"`
`a_1 = "model dynamics that depend on the previous state"`
`a_2 = "An additional model parameter for the use of the LQG controller. In this case this is basically the same as a_0 but in the model used in LQG this a_2 is multiplied by the control value(u)."`
`A_0 = "model dynamics related to measurements that don't depend on the previous sate"`
`A_1 = "model dynamics related to measurements that depend on the previous state"`

The following error parameters may cause instability if all of their values are too small. (for example b_1 and B_2 could be zero but not all of them.)

They are the biggest reason for unstable beahviour of the estimator (You can get Infs and NaNs).

`b_1 = "effect of model based noise/error in the sates"  // Can make state estimation numerically unstable if too small (propability for the system behaving like it really did gets really small. -> precision ends -> unstable behaviour)`
`b_2 = "effect of measurement based noise/error in the sattes"  // Can make state estimation numerically unstable if too small (propability for the system behaving like it really did gets really small. -> precision ends -> unstable behaviour)`
`B_1 = "effect of model noise/error on the measurement" // Can make state estimation numerically unstable if too small (propability for the system behaving like it really did gets really small. -> precision ends -> unstable behaviour)`
`B_2 = "effect of measurement noise/error in the measurement" // Can make state estimation numerically unstable if too small (propability for the system behaving like it really did gets really small. -> precision ends -> unstable behaviour)`
`epsilon_1 and epsilon_2 are random white noise affecting the system.`

By changing these model parameters (they can also be matrises) you can change the physical model of the system.

In the temperatureBoxModel function all the physical values related to the model have been named and commented for easily changing the physical attributes like heat capacity.

# The state estimator

The state estimator theoretically works for any system of the form defined in the temperatureBoxModel function. Therefore cahanges inside it should not be needed.

It is just the implementation of the following matlab code:

`//B_o_B = B_1*B_1' + B_2*B_2' % Noice covariance    `
`//b_o_b = b_1*b_1' + b_2*b_2' % Noice covariance    `
`//b_o_B = b_1*B_1' + b_2*B_2' % Noise dependence (correlation)    `
`//    `
`//sigma = (b_o_B + gamma_t*A_1') * ((B_o_B)^(-0.5)); % Filter gain    `
`//epsilon_th = ((B_o_B * h)^(-0.5)) * (y_th - y_t -(A_0 + A_1*m_t)*h); % Innovation process    `
`//    `
`//m_th = m_t + (a_0 + a_1*m_t)*h +sigma*sqrt(h)*epsilon_th; % conditional mean    `
`//gamma_th = gamma_t + (a_1*gamma_t + gamma_t*a_1' + b_o_b - sigma*sigma')*h; % conditional covariance`
` `
`void discreteStateEstimatorForContinuousSystems(float* m_t, float* gamma_t, float y_t, float y_th, float h, float* a_0, float* a_1, float* A_0, float* A_1, float* b_1, float* B_2, float* m_th, float* gamma_th, float* epsilon_th)`

Inputs are:

`float* m_t, float* gamma_t, float y_t, float y_th, float h, float* a_0, float* a_1, float* A_0, float* A_1, float* b_1, float* B_2`

Where:

`m_t = "last state estimate"`
`gamma_t = "covarince/error of last estimate"`

`y_t = "the old measurement"`
`y_th = "the new measurement"`
`a_0 = "model dynamics that dont depend on the previous state"`
`a_1 = "model dynamics that depend on the previous state"`
`b_1 = "effect of model based noise/error in the sates"`
`b_2 = "effect of measurement based noise/error in the sattes"`
`A_0 = "model dynamics related to measurements that don't depend on the previous sate"`
`A_1 = "model dynamics related to measurements that depend on the previous state"`
`B_1 = "effect of model noise/error on the measurement"`
`B_2 = "effect of measurement noise/error in the measurement"`
` `

Outputs are:

`float* m_th, float* gamma_th, float* epsilon_th`

Where

`m_th = "new state estiamte"`
`gamma_th = "new covariance matrix"`
`epsilon_th = "innovation process" // If the physical model is 100% correct this should be white noise. If it is not you can still tune the physical model to receive better performance. This can thus be used for model identification.`

# The optimal controller (lqg)

The optimal  controller is inside the following function:

`void lqg(float* m_th, float* goalState, float* a_0, float* a_1, float* a_2, float* H, float R, float currentTime, float timeStep, float terminalTime, float* u)`

It calculates the optimal control with LQG and uses inverse time iteration

Inputs are:

`float* m_th, float* goalState, float* a_0, float* a_1, float* a_2, float* H, float R, float currentTime, float timeStep, float terminalTime,`
`H = "importance of each state (penalty in the cost function for error in each state)"`
`R  = "Limit control" (penalty for high controls in the cost function)`
`a_0 = "model dynamics that dont depend on the previous state" // In our case we have to input a a_0 full of zeroes to avoid duplicating the information in a_2 (other algorithms don't use a_2 but need the information)`
`a_1 = "model dynamics that depend on the previous state"`
`a_2 = "An additional model parameter for the use of the LQG controller. In this case this is basically the same as a_0 but in the model used in LQG this a_2 is multiplied by the control value(u)."`
`A_0 = "model dynamics related to measurements that don't depend on the previous sate"`
`A_1 = "model dynamics related to measurements that depend on the previous state"`
`float currentTime = "the current time (for example from the program start)" `
`float timeStep = "The time step used for calculation" (The timestep used for control is a good start choise)`
`float terminalTime = "The time used to start the backwards calcualtion." // This basicaly determines how far in to the future the algorithm looks when finding the optimal control. Small timesteps and bigger end time take more computational power.`

Outputs are:

`float* u = "The best possible control"`

The H and R parameters can make the controller unstable if their values are not in a sensible range. Their values can be found with trial and error (ones or unit matrises are usually a good guesses)

• No labels
No files shared here yet.