c526 - run1

>> CC = AA\BB

CC =

   0.961016219988381  -0.000784249926678
   0.255066453368334  -0.043199693466544
  -0.031415129545827   0.996757195434746
   0.003708535525196   0.000222647704070
   0.015440417834452   0.014078790042442
   0.003761802927405  -0.015195941002062

>>   Ts = 0.003;
>>   A = [ 1 Ts 0 0
        0 CC(1,1) CC(2,1) CC(3,1)
        0 0 1 Ts
        0 CC(1,2) CC(2,2) CC(3,2) ]

A =

   1.000000000000000   0.003000000000000                   0                   0
                   0   0.961016219988381   0.255066453368334  -0.031415129545827
                   0                   0   1.000000000000000   0.003000000000000
                   0  -0.000784249926678  -0.043199693466544   0.996757195434746

>>   B = [ 0
        CC(4,1)
        0
CC(4,2) ]

B =

                   0
   0.003708535525196
                   0
   0.000222647704070

>>   C = [ 1 0 0 0
        0 0 1 0
        0 0 0 1 ];
>>   D = [ 0 ];

>> Qc = [0.0001 0 0 0
0 0.0001 0 0
0 0 100 0
0 0 0 0.001 ]

Qc =

   1.0e+02 *

   0.000001000000000                   0                   0                   0
                   0   0.000001000000000                   0                   0
                   0                   0   1.000000000000000                   0
                   0                   0                   0   0.000010000000000

>> Rc = [0.0001]

Rc =

     1.000000000000000e-04

>> [Kc Sc Ec] = dlqr( A, B, Qc, Rc )

Kc =

   1.0e+02 *

   0.009876447495381  -0.017357360852121   7.872065501825257   1.405361671313067


Sc =

   1.0e+03 *

   0.000941072838029   0.000101937753542   0.004466819115199  -0.001243169339170
   0.000101937753542   0.000031614518702  -0.002610611992193  -0.001307582078682
   0.004466819115199  -0.002610611992193   5.553305327092349   0.408906268501827
  -0.001243169339170  -0.001307582078682   0.408906268501827   0.085575386470330


Ec =

  0.999892837264199 + 0.000000000000000i
  0.985772113354191 + 0.016275068742831i
  0.985772113354191 - 0.016275068742831i
  0.961483335434398 + 0.000000000000000i

>> Qf  = [0.0001 0 0 0
0 0.01 0 0
0 0 0.0001 0
0 0 0 0.01 ]

Qf =

   0.000100000000000                   0                   0                   0
                   0   0.010000000000000                   0                   0
                   0                   0   0.000100000000000                   0
                   0                   0                   0   0.010000000000000

>> 
>> Rf = [ 0.0001 0 0
         0 0.01 0
         0 0 0.0001 ]

Rf =

   0.000100000000000                   0                   0
                   0   0.010000000000000                   0
                   0                   0   0.000100000000000

>> [Kf Sf Ef] = dlqr( A',C',Qf,Rf)

Kf =

   0.628331882843910   2.028282875099887   0.024520421570326  -0.002736846796270
   0.000717123422443   0.175395261554797   0.094956878375175  -0.004262900410578
  -0.000076092629256  -0.047612995298332  -0.000783604143649   0.987160129117135


Sf =

   0.000164568780800   0.000557806899953   0.000007171211397  -0.000000771092476
   0.000557806899953   0.124160925485529   0.001753938331649  -0.000173313391839
   0.000007171211397   0.001753938331649   0.001049568548671  -0.000042332856067
  -0.000000771092476  -0.000173313391839  -0.000042332856067   0.010100367486760


Ef =

   0.950252185799884
   0.905171275679969
   0.382126365000199
   0.009774698606855

************************************************************************
cutoff = 0.1;
Kalman_gain = 0.05;

  Columns 1 through 3

   1.000015708509925   0.010069871950873   0.000062619984133
   0.002969563330821   0.979476523445921  -0.000033413840727
  -0.002397156423669  -1.520459941324681   0.987442556864025
  -0.000147919264830  -0.110129656931570   0.003088277286960
   0.000002486302323   0.001631596056153  -0.000004301441745
  -0.000014890327872  -0.004692784440009   0.000090162421577
  -0.000000283132465   0.004741709972482   0.000004271491317

  Column 4

   0.007242216158399
  -0.003891056646289
  -0.822132154465915
   0.997451248759037
   0.000241475349385
  -0.017275011215169
  -0.021663099244856


A =

  Columns 1 through 3

   1.000015708509925   0.002969563330821  -0.002397156423669
   0.010069871950873   0.979476523445921  -1.520459941324681
   0.000062619984133  -0.000033413840727   0.987442556864025
   0.007242216158399  -0.003891056646289  -0.822132154465915

  Column 4

  -0.000147919264830
  -0.110129656931570
   0.003088277286960
   0.997451248759037


B =

   0.000002486302323
   0.001631596056153
  -0.000004301441745
   0.000241475349385

% state is [ wheel_angle wheel_velocity pole_angle pole_velocity ]

Qc = [0.01 0 0 0
0 0.01 0 0
0 0 100 0
0 0 0 1 ]

Rc = [0.0001]

C = [ 1 0 0 0
        0 0 1 0
        0 0 0 1 ];

D = [ 0 ];

Qf  = [0.0001 0 0 0
0 1 0 0
0 0 0.01 0
0 0 0 1 ]

Rf = [ 0.0001 0 0
         0 100.0 0
         0 0 0.0001 ]

[Kc Sc Ec] = dlqr( A, B, Qc, Rc )

Kc =
8.668641918322800   2.610667153638945 -21.804096846759780  55.965167741466090

[Kf Sf Ef] = dlqr( A', C', Qf, Rf )

Kf'
   0.937914429790626  -0.000013656131981   0.000894705423702
  72.123979231766995  -0.003900328041390   0.185111125258998
  -3.475112983549520   0.000752465373345  -0.050636271742840
   2.631763433678429  -0.000615603273752   1.041237807275785

************************************************************************
cutoff = 0.2;
Kalman_gain = 0.05;

   1.000016205300831   0.009870047640297   0.000057147702245
   0.002955415441754   0.968545067539203  -0.000022395588531
  -0.002082735507993  -1.184426613222790   0.987236860724156
  -0.000176395019264  -0.113839268131090   0.003191349460729
   0.000004124352763   0.002821314127368  -0.000003812826857
  -0.000014726476968  -0.006017968002845  -0.000105971672195
  -0.000017760920893  -0.005299411225081   0.000005690268448

  Column 4

   0.008950505383015
  -0.000913551849316
  -1.159076871259259
   0.970255090784234
   0.000088951001520
  -0.018619837670189
  -0.029903415169891


A =

  Columns 1 through 3

   1.000016205300831   0.002955415441754  -0.002082735507993
   0.009870047640297   0.968545067539203  -1.184426613222790
   0.000057147702245  -0.000022395588531   0.987236860724156
   0.008950505383015  -0.000913551849316  -1.159076871259259

  Column 4

  -0.000176395019264
  -0.113839268131090
   0.003191349460729
   0.970255090784234


B =

   0.000004124352763
   0.002821314127368
  -0.000003812826857
   0.000088951001520

Kc
   13.4152429314736   4.4122045057493  -152.3395979186784   3.1608414827951

Kf'
   0.889690554428292  -0.000009759213625   0.000929768074359
  59.045118931244389  -0.002724946310649   0.188822795200805
  -2.494628160431318   0.000697085447092  -0.066943331235042
   2.881178109453300  -0.000816680510472   1.052298844617387

************************************************************************
cutoff = 0.05;
Kalman_gain = 0.05;

   1.000005995861639   0.003854334780728   0.000077097447388
   0.002991200882357   0.994091835855259  -0.000058856997727
  -0.001806623262416  -1.175648249937205   0.987505562398797
  -0.000068517559534  -0.052016450992636   0.002589640875443
   0.000000604092731   0.000394748080902  -0.000001891990896
  -0.000029570211053  -0.017337680945114   0.000105347647661
   0.000031511435555   0.022477129990112   0.000009769493726

  Column 4

   0.004671923924044
  -0.002567313683939
  -0.505088432358697
   0.993714334914859
   0.000101847652389
  -0.009733705490300
  -0.010124325485728


A =

  Columns 1 through 3

   1.000005995861639   0.002991200882357  -0.001806623262416
   0.003854334780728   0.994091835855259  -1.175648249937205
   0.000077097447388  -0.000058856997727   0.987505562398797
   0.004671923924044  -0.002567313683939  -0.505088432358697

  Column 4

  -0.000068517559534
  -0.052016450992636
   0.002589640875443
   0.993714334914859


B =

   1.0e-03 *

   0.000604092730571
   0.394748080901646
  -0.001891990895674
   0.101847652389112

Kc =
4.854751907269774  14.230628899015420  45.134774760715338  -6.702683874745908

Kf' =
  0.940217862055481  -0.000014571672739   0.000617788282018
  72.800971863850364  -0.004119958529980   0.141392118031835
  -3.788467605381915   0.000945841956731  -0.040223242011460
   1.762890491639427  -0.000476016381483   1.015143587980286
