Kalman Gain: Error 41653 in Control Design and Simulation: Number of Rows in G does not equal the number of states in the system model

[Lexicondi]

Hey Barp, 

I think I am facing a similar challenge. Trying to implement an LQG controller as attached in the following VI.

I am getting an error, 

      NI_CD_Stochastic Systems.lvlib:cd_verify m Q R N.vi<append>
<b>Complete call chain:</b>
NI_CD_Stochastic Systems.lvlib:cd_verify m Q R N.vi
NI_CD_Stochastic Systems.lvlib:CD Verify Noise Model.vi
NI_CD_State Feedback Control.lvlib:cd_Kalman Gain (Stochastic).vi

Will appreciate your help!

[Lexicondi]

When I change the Kalman Gain VI to a discrete kalman filter as in the image below

the code runs fine on the RIO target but neither the observer gain nor Kalman filter gain are computed. The VI is attached below,

When I double blick the discrete KF vi, I see the following in the error display:

>NI_CD_Stochastic Systems.lvlib:cd_verify m Q R N.vi<append>
<b>Complete call chain:</b>
NI_CD_Stochastic Systems.lvlib:cd_verify m Q R N.vi
NI_CD_Stochastic Systems.lvlib:CD Verify Noise Model.vi
NI_CD_Stochastic Systems.lvlib:CD Construct Noise Model (Numeric).vi
Soft_Robot Model Two States.vi

Wondering what I could possibly be doing wrong.

Will defo appreciate your help.

[Barp]

Some things to consider:

a. The A,B,C,D matrix on your file was not saved. You have to enter and make current values default to make it fixed.

b. To have a better speed, you should try to create the State-Space Function and the controller 'outside' the loop, since it seemed that the controller is not updating...

c. Your implementation of the state feedback control seemed to be misleading... you should use the function y = -Kx and then, only the 

d. To know which size of variable to use (E{v}, E{w}, Q, R, N, etc), you can try to open the "Discrete Kalman Filter" , Select Stochastic SS, and then, use the dimensions to tell you want the parameters are suppose to be (The UI has a way to define those features).

e. The QRN for LQR is different then the QRN for Kalman. Yes, it is confusing, but they are different sizes and it is pretty easy to be confused.

Anyway, this is just to allow you to make this to get the right size.

I hope this helps a bit...

[Lexicondi]

Thank you!

I appreciate the time and response. A couple of questions though.

a. How do you make the state space matrices default?
b. The values of the state space matrices are already created outside the loop.
c. I cannot seem to be able to find the polymorphic instance u = -Kx on the state feedback palette. Can you post the full path where it can be found please?
d. I have nicely implemented the KF as suggested.
e. This is where I get confused. Since my state space is 2 x 2, I set Q to be a 2 x 2 matrix and wire a constant "Output Weighting, Dim Q = Nx" to the discretized LQR vi. R is a 1 x 1 matrix since my output is 1. E{v}=E{w}=0 as always. When I run, the KF doesn't give any errors but I get issues from the discretized LQR:

                                       Error -41511 occurred at NI_CD_State Feedback Control.lvlib:cd_Discretized Linear Quadratic Regulator.vi
                                      This error code is undefined. Undefined errors might occur for a number of reasons. For example, no one has provided a description for the code, or you might have wired a                                                 number that is not an error code to the error code input.
                                          Additionally, undefined error codes might occur because the error relates to a third-party object, such as the operating system or ActiveX. For these third-party errors, you might                                        be able to obtain a description of the error by searching the Web for the error code (-41511) or for its hexadecimal representation (0xFFFF5DD9).

when I probe the LQR error wire in the attached VI.

On the front panel, the same error says,

                                            Error Code: -41511
                                            NI_CD_State Feedback Control.lvlib:cd_Discretized Linear Quadratic Regulator.vi<append>
                                            <b>Complete call chain:</b>
                                           NI_CD_State Feedback Control.lvlib:cd_Discretized Linear Quadratic Regulator.vi
                                           Soft_Robot_Model2.vi

e. Last question, I followed your CDEx LQG with Linear Simulation vi example and I tried to put a compensator to merge both the KF gains and LQR gains before I used the Ackerman vi to get the feedback gain which I now use to compute the control law (as in the attached vi). I feel I need some sort of series/parallel combination of each gain (KF and LQR) before I compute the control law as you guys did in the simulation example I pointed out earlier. But I am not sure how to extract the state feedback gain from the compensator VI since it returns a state space structure. Is this correct? Or should I stick with the previous method where I used the LQR gain alongside the KF estimate in computing the control law.
Pardon my many questions but I would be glad to read your suggestions further.

Sincerely,
Ola

PS: in case, the state matrices do not show up, I have attached the Matlab idss struct file that contains A, B, C, D, and K {=stochastic process noise model} since I am unyet familiar with saving LabVIEW SS matrices as default yet. Simply call each matrix as FreeForm.a for the A matrix for example after you load the .mat file in your workspace.

A =   1.01         -0.1716                     B = [-1.706e-07   6.469e-07]^T,      C = [1.089  -0.03769],           K = [0.8307    -0.1412]^T,                 Sampling Time = 0.1667s                    H = 0.
        0.001556  0.9782

EDIT: Oops! Your server rejected the .mat file. The matrices are copied out above. 

[Barp]

Here are some responses:

a. You type the numbers in the matrix, then you right click to it, go to "Data Operations" and select "make current value default". Notice that you have to right click at the border of the matrix, and not in the middle. OR, the easiest way would be to fill all of the controls and indicators properly and the go to Edit menu and select "Make Current Values Default". Then, to be sure you have the values, close the VI and open again. If you did not do the procedure correctly, the matrices will be empty.

b. Well, the matrices are outside, however, in dataflow, the 'creation' of the state-space model is done inside the loop. So, the procedure to 'create' the state-space model is repeated over and over, although you would not change the values. Here is an example of what I meant below. However, if for some reason you need to change the values while the loop is running, then the matrices and the creation would have to be inside the loop.... 

c. the function is located at: Control & Simulation >> Control Design >> Implementation folder.

d. that seemed reasonable, but you have to be careful with somethings. First the size of the noise. This is defines the size of G and H. If those matrices are empty, then you will have an error:

Then, when defining the noise model statistics, notice also that you have to specify the dimensions based on the noise size. Here is the size that should be entered for a noise of size 1. Notice that, although E{w} and E{v} are zero, then need to have 'dimension of 1' as zero to avoid generating errors.

So, the size of those matrices are based on the noise size and the output signal, and not on the states or inputs.

Again, the QNR for Kalman is different than the QNR size for LQR. We probably should have call those variables differently. 

e. Unfortunately, that example is a not one that explains how to do a LQG controller and we really don't have a full example with LQR and Kalman together. However, I select some examples that would help you to learn Kalman and LQR on our 

Good Kalman Examples:

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Algorithms\Extended Kalman Filter\Localization with Extended Kalman Filter.vi

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Case Studies\Electric Circuits\CDEx Discrete Recursive Kalman Filter for RL Circuit.vi

Good LQR Examples (with different ways to implement the controller):

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Case Studies\Mechatronics\Inverted Pendulum\Linear Inverted Pendulum Simulation.vi

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Case Studies\Mechatronics\Cart-Pole\CDEx Cart-Pole Control and Simulation.vi

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Case Studies\Mechatronics\Ball Beam\Ball Beam.vi

So, after you are able to 'estimate' the states (Kalman examples), then you just need to apply the gain into the -Kx to calculate the controller.

One last thing: what is the T in here? B = [-1.706e-07   6.469e-07]^T

[Lexicondi]

Thanks! The T is just a transpose of vector B. 

I will try your suggeestions and revert to you.

[Lexicondi]

Thank you indeed!

I have cleaned up the block diagram a bit based on your suggestions. State Space and Discretized LQR are now outside the timed loopp but I still get the error -41511:

                         NI_CD_State Feedback Control.lvlib:cd_Discretized Linear Quadratic Regulator.vi<append>
                         <b>Complete call chain:</b>
                         NI_CD_State Feedback Control.lvlib:cd_Discretized Linear Quadratic Regulator.vi
                         Soft_Robot_Model2.vi

Also, probing the Q and R wires for the LQR matrix outside the timed loop indicates that part of the program is not executing. So is the State Space Wire before the Timed Loop.

Maybe you have some ideas?

[Lexicondi]

I tried to calculate the desired LQR gains offline. So I deleted the LQR vi and the Timed loop has the discrete KF vi alone running. Thing is every section of the code is working but the KF estimator.

The vi is attached below.

[Lexicondi]

Updated VI's are available here:

https://github.com/SeRViCE-Lab/ACCCodes/blob/master/Soft_Robot_Model2.vi   for the hard-coded gains

and 

https://github.com/SeRViCE-Lab/ACCCodes/blob/master/Soft_Robot_Model_Conf.vi for the mQRN-based vi.

Please take a look when you have the time. 

Thank you!

[Barp]

The error I've got (I had to remove your FPGA code) was the 'covariance matrix' size. The P(k|k-1) has to be 'squared' and positive definite. The value hard coded on the Kalman Filter was not squared. As soon as I made it a square matrix, then there was no error anymore.

Please also see this example below:

C:\Program Files (x86)\National Instruments\LabVIEW 2015\examples\Control and Simulation\Algorithms\Kalman Filter\1D Kalman Filter Encoder and Accelerometer.vi

In here, this shows a 'framework' where you can have a 'simulation' of the plant you are trying to control and the separate loop to define the estimation (and you can add control). This could help you to better tune your controller too.