OOOOOOOOOOOI

!"

^matnx

p,

vector scanf("iglE'',&w);

Eor (i=O;j

^<16

^n'

⁺⁺⁾

scanf{"iglf,b,&p[ ±

]);

for(i=O;j<4;i++}

scanf("klf",&q[i]);

for(i=O;i<4;i++)

scanf("Z1f'i,&c[i]};

1'

pr

±nt p,q,c ^tf

matprn{&p[O],4,4};

matprn(&q[O],4,1);

matprn{&c[e],1,4);

lt1.1-E=!tlt/+

CcPcPtp

cP"n-1

t!'f*!t!k!+!

q,c

.w;

read

^"1

matcpy(&c[O] 4,&cO[Ol,1,4};

matzero{&e[O] ,4};

for{i=O;i<n;l++}

{

^for (j=O;j<n;

]'^++}

"(e+n'i+j)='{cO+j);

matmul{& ¢ O[O],1,4,&p[O],4,4, matcpy{&dul[O],1,4,&cO[O],1,

}

fprintf{stderr,"Xn Matrix

matprn{&e[O],4,4)J

1'

^{detE !=}

O ^t!

dummy=matdet(&e[O],4,4);

fpr ±ntf{stderr,"Xn detE ⁼

^g15 iE(fabs(dummy}

^<

epsl ) {

fprintf(stderr,''Xn detE Z15,

^exit(1);

}ret=rnatinv(&e[O],4,4.

if(ret!=1)

{

iprintf(stderr,"Xn 1

inverse

exit(1>;

}matcpy(&du2[O1,4,4,&e[O],4,4);

!"

d=invE"col[e]

f'

d=invE" O ^tf

!.

.

^tl

!k

.

^+!

!'

O ^tl

!t

¹

^de!

matmul(&e[O].4.4,

.!

&dul[O].1.4);

4);

E: ^ii);

.101g",

71g ^<

dummy>;

%15.

&du2[O],4,4);

&cole[O],4,1,

1' T=[P"n-1'd

...

Pd d] Y

for(

±=OJti<n;i++)

{

for(j=:O;j<n;j++) '(t+n+j+n-i-1}=t(d+j);

71gXn'b

matrlx

&d[O],4,

,fabs(dumy)

can't

1);

excute

,eps1);

.Xn'T)i

45

　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　 　

｝

： 　

： 　

； 　

， ，

　

： 　

； 　

■ ．

　

： 　

＝ 　

：

　

，

，

　

： 　

： 　

： 　

： 　

：

　

・

，

　

，

「

　

；

　

，

．

　

．

，

　

＝ 　

： 　

： 　

＝ 　

： 　

：

　

，

．

　

： 　

： 　

：

　

．

■

　

＝ 　

： 　

； 　

： 　

： 　

；

　

．

．

　

：

8901234567890123456789012345678901234567 7788888888889999999999000GOOOOOO11111111 1111111

⊥

11111111111111222222222222222222

0QOOOOOOOOOOOOOOOQQOOOOOOOOOOOOOOOOOOQOOma 辷mul 〔^＆

p

［0_］

，

⁴

，

⁴

，

^＆d匚0_］

，

⁴

，

¹

，

^＆du2 ［OL4

，

¹）；

mat 二cpy （＆

du2

_［

0

_］

， ⁴ ， ¹ ，

^＆

^d

［

0

_］

， ⁴ ， ¹

）；

｝

／^★　　　TO

＝



inv

　

T

^t ／

re

仁

＝ _ma

_し

inv

（＆t［

O

］

， 4 ，

4

，

δ

⊆

tO ［

O

］

， 4 ，4

）；

if ｛^{re 七}

1

^＝1）　 ｛

　　　fprintf （^sしderr

，

^{1へ n}　2　

inverse

　^{ma しrix 　can}

’

_辷

　^excute

．

＼ⁿ

闘

_〉

；

　　　^{exi ヒ}（

1

_）； 　 　　 ｝

fprintf （^{s し}derr

、

¹へⁿ 　Mat二rix 　T：II＞；

matprn （＆

t

_［O］

，

⁴

，

⁴＞^；

ma

しprn ^（＆しO［O］

， ⁴ ， ⁴

）；

／^★ 　

PO ＝

_invT ^★

_P

_畑 ^＊_／

mat 二

mul

（^＆tO 〔

0

］

． 4

，

4

，＆p ［

0

］

， 4t4

，＆

dul

［0］，

4

，

41

； mat 二mul （＆du1 〔OL4

，

⁴

，

^＆

^t

^二［0］

，

⁴

，

⁴

，

^＆du2 ［0］

，

⁴

，

⁴）； matcpy （＆du2 ［0］t4

、

⁴

，

^＆

p

【0］t4

．

⁴）；

／^★ 　

qo ＝

_invTk^de_／

matmul （^＆

tG

_［0］

，

4

，

4

，

^＆_q［0】

，

4

， ¹ ，

^＆du1 ［

0

_］t4

， ¹

｝；

ma しcpy （^＆dul ［0］

，

4

，

1

，

^＆_q ［OL4

、

1）；

／^t　　　Co　

＝

_cdeT^t _／

matmu1

_（＆

c

_［OL1

，

4

，

^＆t［0］

i4 ，

4

，

^＆du2 ［O］r1

，

4）； ma 七

cpy

（＆du2 ［0ユt1

，

⁴

、

^＆c ［O］

，

¹

，

⁴）；

fprintf （^st：derr

， ’

へⁿ mat ：prn （^＆P［

0

_］

， ⁴

，

4

_）；

fprintf

（s七

derr 厂

¹へ n ma ヒprn ^（＆q^［0］

，

4

厂

1）≡

fprintf （^st二derr

， ’ 圏

_＼n

ma ヒ

prn

〈^＆c［O］

，

1

厂

4）；

Marix

　

^PO

^：

”

）；

Vector

qo ^：

”

_｝

；

Vector 　^{co ：11}）；

printf ^（

”

_％lf＼n

’ ：



，

^w ）；

for （

i＝

0；i^＜16；

i

^{＋ ＋}）

　　　　pr^土ntfC

’

_％

₁₅ ． 101E

＼n

潤

fQr （ゴ

＝

_0；ゴ＜4；ゴ^{＋ ＋}）

　　　　

printf

（

”

_％15

．

101E ＼^{n ，鬮}

fQr （」

； OFj

く

4F

ゴ^{＋ ＋}）

　　　　print ^二f（^Il髦15

．

101E ＼ⁿ

悶

，

_P ［i］）｝

， q

［」］）；

，

^c ［ゴ^］）；

付 録

H

　 連 続 時 間系^{か ら}離 散 時 間系^{へ の} 変換 0001 ^：

0002

：

0003 ：

OOO4 ：

0005

^： 0006 ^： 0007 ^： 0008 ：

0009 ；

0010 ：

OOII ：

0012

：

0013 ；

OO14 ^：

0015

^： 0016 ：

program 　

c2d

implici し　^{real ★}8　 （^a

−

_h

，

^o

−

_{z ）}

co   on 　p ^（4

厂

4｝

，

_qG （4

，

4）ra （4

，

4）

，

b （4

，

1）

，

^c （1

，

4＞ co   on d （4

，

4＞

、

^e （4

，

4）

，

fil ｛4

、

4｝

， g

（1

，

1）

co   。nq （

4 、

1｝

，

du！（

4 ，4

）

， du2

｛4

， 4

｝

，

du3 （4

，

1） epsl

＝

1

．

OE

−

₁₅

open

（

10 ，

read （10

，

^de｝ read （10

，

^t 》 read （10

，

^t ｝

τ

ead

（10

，

^★｝

read （10

，

^★｝

file ＝圏 D1 ‘

_）

｛〈^a （i

，

」）

，

」

ニ

₁

，

4）

，

i

＝

₁

，

4｝

（b （i

，

1｝

，

i

＝

₁

，

4）

（^c （1

，

i＞

， i＝

1

，

4） し

W

46

OO17:OO18:OO19:O020:O021:O022:O023:O024:O025:O026:O027:O028:O029:O030:O031:O032:O033:O034:O035:O036:O037:O038:O039:oo4e:O041:O042:O043:O044:O045:O046:O047:O048:O049:O050:oesl:O052:O053:O054:O055:O056:O057:O058:O059:O060:c

O061:c

O062:c

O063;c O064:c Oe65,c O066:c O067:c O068:c Oe69:c

O070:cO071:cO072:cO073:cO074:O075:O076:

close(10)

^cal1 matprn(a,4,4) ^call

matprn(b,4,1)

^cal1 matprn{c,1,4}

call

matmuls(a,4,4,t,dul,4,4)

^cal1

matcpy{dul,4,4,a,4,4}

^caU matidn{d,4,4)

^caU matidn{p,4,4) ^{caU mat ±}dn{qO,4,4)

fk=O.O

i=o 100 cont ±nue fk=fk+1.0

i=i+1

^caU matmul(a,4,4,d,4,4,dul,4,4) du=1.0ffk

cal1 matmuls(dul,4,4,du,du2,4,4}

^call matcpy(du2,4,4,d.4,4) du=1.0f{fk+1.0}

^cal1 rnatmuls{du2,4,4,du,e,4,4) ^cal1 rnatadd(p,4,4,d,4,4,dul,4,4) ^call matcpy(dul,4,4,p,4,4)

^cal1

rnatadd(qO,4,4,e,4,4,du2,4,4)

^cal1 matcpy{du2,4,4,qO,4,4) ^call absum(d,4,4,el)

write(k,101) ±,el

if{el.lt.epsl} goto 102 goto 100

101 format(i5,e18.10}

102 continue

^{write(t,S '}

Matrix

P'

^ca]1 rnatprn(p,4,4) ^cal1 matmuls(qO,4,4,t,dul,4,4) ^cal1 matmul{dul,4.4,b,4,1,q,4,1)

^{write(',') E} Vector q'

^cal1

matprn(q,4,1)

^{call rnat ±}dn{fi1,4,4}

^call matsub(ti1,4,4.p,4,4,dul.4,4}

'

icon=-1

^cal1 matinv(dul,4,4,du2,4,4,i ¢ on}

^{wr ±}te('.-) ^'

^Matinv

return',icon

pause

if(icon.ne.O)

then

^{write(','} '}

^Matrix

^{not regular} pause

stop endif

^cal1 rnatmul(du2,4,4,q,4,1,du3,4,1}

^cal1 rnatrnul{c,1,4,du3,4,1,g,1,1) write(',106}

g{1,1)

'

open(1O,fUe='D2')

write{10,106} w

return'

47

O077:oe7s:O079:O080:O081:O082:O083:O084:O085:O086:O087:O088:O089:

Td-$km

OOOI:OO02:OO03:OO04:OO05:ooe6:OO07:OO08:OO09:OOIO,OOII:OO12:OO13:OO14:OO15:OO16:OO17:'OO18:OO19:O020,O021:O022:O023:O024:O025:O026:O027:O028:O029:O030:O031:O032:O033:O034:O035:O036:O037:O038:O039:O040:O041:O042tO043:

103 104 105 106

do 103 i=1.4

do 103

j=1,4

write{10,106) p(i,j>

do 104

i=1,4

write(10,106)

q{

±,1}

do

¹⁰⁵

i=1,4

write(10,106)

^c(1,i) close(10)

forrnat{e18.10}

pauseend

iJde

vffireik

program Riccati

implicit

^real"8 (a-h,o-z)

^cornmon a(5,5},a1(1,1),b{5

^common

e(5,5},h(5,5},g(1,

^common p(4,4},p1(5,5),q(4

^cornmon dul(5,5),du2{5,5>,

^common eul(1,4),eu2{1,1}

character'64 filel,file2

,1),bl(1.5}.c(1,4}.cl(1,5) 5),gain(1,4)

,1},ql(5,1),wO(5,5}

du3(1.5>.du4(5,1}

open(1O.file='Init'}

^read(10,')

f

±

lel

^read(10,t) f±le2

^close{10}

^{write{',-} '}

^F

^±lel ^',filel

write{",'} ^'

File2

^',file2

open(10.file=filel) read(10.'} w

^read(10,'}

((p{i,j),j=1,4),

^read(10.')

(q{i,1).i=1,4}

^read(10,'}

(c(1,

^±

),i=1,4}

close(10}

^{write(',') '} We ±

ght

^{',w caU}

matprn(p,4,4}

^cal1

matprn(q,4,1) ^cal1 matprn(c,1,4) do 100 i=1.4 100 cl(1,D=c{1,i}

clCl,5)=O.O

^call

matzero(pl,5,5) do 101 i=1,4

do 102

j=1,4

pl{i+1,j+1)=p(i,j}

102

continue

101 continue

^cal1 matmul<c,1,4,p,4,4,eul

do 103

i=1,4

i-1,4)

,1.4)

48

O044:O045:O046:O047:O048:O049;O050:O051;O052:O053:O054;O055:O056;O057:O058;O059:c

O060:O061:O062,eo63:O064:O065:O066:O067:O068:O069:O070:O071:O072:O073:O074:O075:O076:O077:O078,O079:O080:O081:O082:O083:O084:O085:O086:O087:O088,O089:O090:c

O091:O092:O093:cO094:c

O095:O096;O097:O098;O099:OIOO:OIOI:c

OI02:OI03:

103

104

109

105

110

pl(1,i+1)=eul(1,i) pl(1.1)=1.0

write{+,t) ^t

Matrix Pl

^'

call matprn{pl,5,5)

caU matmul{c,1,4,q,4,1,eu2,1,1)

qlC1,1}=eu2(1,1}

do 104 i=1,4 ql(i+1,1)=q(i,1)

write(',") ^'

Vector

q ^'

call matprn{ql,5,1}

call mattrn{cl,1,5,du4,5,1>

cal1 matrnul(du4,5,1.cl.1,5,wO,5,5)

ret=O.O

cal1 absum(wO,5.5,ret)

do 109

i=1.5

ret=ret+dabs(wO(i.i)}

cal1 matcpy(wO,5.5,h,5,5)

i=owl=O.O

epsl=1.0E-7

cal1 matprn(wO,5,5) call matprn(h,5,5)

write{ ^±_,t} ret

pause

^' Hit retuxn key

to

vontinue ⁱ

contlnue

i=i+1

call matcpy(h,5.5,e,5,5)

call matrnul{h,5,5.pl,5.5,dul,5,5}

cal1 mattrn(pl,5,5,du2,5,5}

call matmul(du2,5,5,dul.5,5.a,5,5) caU mattrn(ql.5,1,du3,1,5}

call

matmul{du3,1,5,dul,5,5,bl,1,5) cal1 matmul(h,5,5.ql,5.1,du4,5,1) call mattrn(pl.5,5,du2,5,5}

cal! rnatmul(du2,5,5,du4,5,1.b,5,1) call mattrn(ql,5,1,du3,1,5)

¢al1 matmul(du3,1.5.du4,5,1,al,1,1) wl=al(1,1)

w2=wl+w

call matmuls(bl,1,5,1.01(al{1,1)+w),g,1,5)

do 110

j=1,5

g(1,j)=blCl.j}!<al(1,1}+w)

caU rnatmul(b,5,1,g,1,5,dul,5,5) cal1 matsub(a,5,5,dul,5,5,du2,5,5)

call matadd(du2,5.5,wO,5,5,h,5,S}

call matsub{h,5,5,e,5,5,dul,5,5)

call

matcpy{dul,5,5,e,5,5) call

absum(e,5,5.eO)

eo=o.o

do 111

j=1,5

49

el04rOI05,c

OI06:c

OI07:OI08:OI09:OllO:Olll,Ol12:Ol13:Ol14:Ol15:Ol16:Ol17:Ol18:Ol19:O120:O121:O122;O123:O124:O125:O126:O127:

111

106107

108112

eO=eO+dabs(dul(j,j)) eO=eO!ret

write{',') eO

if{mod(i,1eO).eq.O}

then

write(".107)

i.g(1,1},g{1,2}.g(1,3),g(1,4).g(1

endif

ii(eO.lt.epsl> goto

106

goto

105

,contlnue

formatCi6,el8.1O.el8.1O,el8.1O,el8.1O.e18.1O}

write{t,t)

t

_{LQ Gain}

_Vector '

call

matprn{g,1,5) open(1O.iile='CGain"}

do 108 i=1.5

^{wr ±}

^te{10,112)

g(1,i) format{e18.10)

pause ^'

^Hit

^return

^{key to}

^end program'

end

,5)

N$Xps

OOOI:OO02:OO03:OO04:OO05:OO06,OO07;OO08:OO09:OOIO:OOII:OO12:OO13:OO14:OO15:OO16:OO17:eols:OO19:O020:O021:O022:O023:O024:O025:O026:O027:eo2s:O029:O030:O031:O032:

x

i=

･y fnE3asov }

^an v-vg tz

program

obsever

impli ¢ it real"8

(a-h,o-z)

common

p(4,4),q(4,1),c(1,

^common dul(4,4},du2(4,4),

^common du5{4,1),du6(4,1},

^comnon x(4,1),xO(4,1}

character*64 filel,f ±

le2,

character'1

tab

tab=char(z'O9')

,

open<1O,file=;Init'>

read(10.') filel

read{10,"} f^±

^Ie2

read{10,'} file3

read{10.')

file4

read{1O,'} num

read{10,*}

t

±

close(10)

write(".') ^'

Filel

^'

write<t,') ^'

File2

^b

write<",') ^'

File3

ⁱ

write(',+) ^' File4 ^'

write{','> ^'

Data

write(k,'} ⁱ

Time

^b

open(10,fUe=filel)

read(10,'} w

read(10,t)

' '

read(10,'} (q(i,1),

numberi

'

4},gain{4 du3(4,1), du7(1,1}

Iile3

,filel

,file2,file3,file4

,tl

'

C{p(i,]},j=1

i=1,,4)4)

num

'

,1}.g(1,4)

du4(4.1)

file4

,,1=1,4)

50

O033:O034:O035:O036:O037:O038:O039:O040;O041:O042:O043:OD44:O045:O046;O047:O048:O049;O050:O051:O052:O053:O054:O055;O056:O057:O058:O059;O060:O061:O062:O063:O064:O065:O066:O067:O068:O069,O070:O071:O072:O073:eo74;O075:O076:O077:O078:O079:O080:O081;O082:O083:cO084:O085:O086:O087:O088:O089:O090:O091:O092:

read{10,') (c{1,D,i=1,4>

close(10}

open(10,file=file2}

read(10,'>

(gain{i,1),i=1,4)

close(10)

open(10,file='CGain')

read(10,')

gO

read(10,'}

(gCl,i),i=1,4)

close(10)

write{",') ^' Matrix P ^'

cal1

matprn(p,4,4>

write(',t) ^'

Matrix Q

^'

cal1 matprn(q,4.1)

write(','}

^' Matrix C ^'

cal1 rnatprn(c,1,4)

wTite<',')

' _{LQ Observer}

call matprn(gain,4,1)

write(t,"} ^`

LQ Contro]er

cal1 matprn(g,1,4)

call matzero{x,4,1}

call matzere(xO,4,1) r=loo.o

yestO=O.O

pause ^'

^H

^±

^t

^return key

open{10,file=file3) open(11,file=fUe4)

do 100 i=1

read(1O

read(10,'} u

callcaUcallcallcallcallcall

,num,t)

y

Gain

ⁱ

Gain

ⁱ

to

continue ^]

matmul(gain.4,1,c,1,4,dul,4,4) rnatsub(p,4.4,du1,4,4,du2,4,4}

matmul(du2,4,4,x,4,1,du3,4,1}

matmuls{q,4,1,u,du4,4,1) matmuls(gain,4,1.y,du5,4,1) matadd(du3,4,1,du4,4,1,du6,4,1}

matadd{du5.4,1,du6,4,1,x,4,1}

^call

matmul(g,1,4,x,4,1,du7,1,1) uO=uO+gO"(r-yest)-du7(1,1)

call matmul{p,4,4,x,4,1,du3,4,1) call matmuls(q,4,1,uO,du4,4,1) call matmul(c,1,4,x,4,1,du7,1,1}

yest=du7(1,1)

write(",103) ^write{t write<11.1O2)

&

&100

continue

103101

1',tab,y,tab,yest,tab,dabs{y-yest)

101) i.y,yestO,dabs{y-yest}

dble{i)+ti,tab,y,tab,yest,tab,dabs(y-yest),

tab,x(1,1},tab,x(2,1),

tab,x(3,1),tab,x(4,1}

closeCIO) close(11}

EormatG5,al.el8.1O,al,el8.

iormat(i5,el8.1O,el8,1O,el810.al,.10)e18.10)

51

O093:O094:O095:O096:O097:

102 format(f6.2,al,e18.

&

al.e18.10

pause ^'

^H

^±

^t

^return

end

10,,alkeyal,e18.

,e18.10

to

end

10,al,e18.10,al,

,al,e18.10)

progTam ⁱ

e18.10'

ItriXV

ドキュメント内 現代制御理論による車体屈折式車両の自動操向制御 (ページ 45-52)