lftjet7vs-7t

OOOI:OO02;OO03:+

M- lftjet7vs-7t

program ^observer

implicit real'12

(a-h,o-z)

common a(4,4),b(4,1),c(1,4) common at(4,4},ct(4,1)

cornmon p(4.4},p1(4,4),q(4,4),ql<4,4},wO(4,4}

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

common du3(4,1),du4{1,4).du5(1,1),du6{4,1)

common

du7(4,4),du8(4,4},du9{4,4)

common dulO(4,4),dull(4,4).du12{4.4}

character"64 f^±

lel,file2

epsl=1.0E-7

open(10,Eile=iInit')

read(10,de} Eilel

read{10,")

fUe2

close(10}

wrtte{',') [ilel

write(k,') file2

open(10,fUe=tilel)

read(10,'} w

read{10,')

{{a(i,j).j=1,4},i=1,4)

read{10,') {b(i,1),i=1,4)

read(10,'} (c(1,i},i=1,4)

close{10}

open(10,iile='Weight')

read{1O,') w

read(10,") s close(1O)

write{',') ^'

Weight

write<',105} w

write(',le5) s

cal1 rnatprn(a,4,4)

cal1

matprn{b,4,1) cal1 rnatprn(c,1,4)

caU matidn(ql,4,4>

call matidn(q,4,4}

call matmuls{ql.4,4,w,q,4,4) s=1.0

call matidn(p,4,4}

call matidn{pl.4,4>

call

mattrn(a,4,4,at,4,4)

call mattrn(c,1,4,cL4,1}

58 O052;c

O053:c O054:c

O055:c

O056:c O057:c

O058:c

O059:c O060:c

O061:O062:c

O063:c

O064:c

O065;c O066:c O067:c

O068:c

O069:c O070:c O071;c O072:c O073:c

O074:O075:O076:O077:O078:O079:c

O080:c O081:O082:c O083:c O084:O085:c O086:c O087:O088:c

O089:c

O090:O091:c

O092:c

O093:O094tc O095:c O096;O097:c O098;c O099:OIOO:c OIOI:c OI02:OI03:c Ol04:c OI05:OI06:c OI07;c

OI08:OI09:OllO:Olll:

201 200

100

do 200 i=1.4

do 201

j=1,4

ddd=a(i.j)

write(',")

j,i,a{i,j),i,j

at{j,i)=ddd

^writeC',') j,i.at(j,i),i,j

'

contlnue

ct(i,1)=c(1,i}

,

contlnue

write(',') ^' Matrix P;

cal1 matprn(p,4.4)

write('.'} ⁴

Matrix Pl'

cal1 matprn(pl,4,4}

write(',') ^' Matrix

Q'

call rnatprn{q.4,4}

write('.') ^'

Matnx

Ql'

cal] matprn{ql,4,4}

write(',') ^'

Matrix At'

call

matprn(at,4.4}

write(',D ^' Matr ±x Ct'

call

matprn{ct.4,1}

pause

i=ocontlnue

caZ1 matrnul(a,4,4,p,4,4,dul,4,4)

write(',') ^' Matr ±x 1'

cal1 matprn{dul,4,4}

cal1

matmul{dul,4,4.at,4,4,du2,4,4>

write(",') ^'

Matr

±x 2'

cal1 matprn{du2,4,4)

cal1 matinul{dul,4,4,ct,4,1,du3,4,1) write(de,S) ^"

Matrix

cal1 matprn(du3,4,1) cal1 matmul(c.1,4,p,4,4,du4,1,4>

write{*,') ^'

Mat=ix

cal1 matprn{du4.1,4)

call rnatrnul(du4,1,4,ct,4,1,du5,1,1)

write{',') ^'

Matrix

write(',') du5{1,1}

cal1 matmul(du4,1,4,at,4,4,du6,1,4) write('.') ^" Matr ±x 6'

cal1

matprn(du6,1,4}

cal1 matmul(du3.4,1,du6,1,4,du7,4,4) write(S,S) ^" Matrix

^7'

call

matprn{du7,4,4)

cal1

matmuls{du7,4,4,1,O1{s+du5(1,1))

write(',')

' _Matr

_±x _8'

cal1 matprn{du8,4,4)

call matsub{du2,4,4,du8,4,4,du9,4,4}

write(+.t> ⁱ

Matrix

cal1 matprn(du9,4,4)

call matadd{q,4,4,du9,4,4,dulO,4,4)

write(t,') ^'

Matrix

10b

cal1 matprn{dulO,4,4}

call matsub(p,4,4,dulO,4,4,pl,4,4)

cal1 absurn(pl,4,4,eO)

call matcpy{dulO,4,4,p,4,4) call matcpy{dulO,4,4,pl,4,4)

'du8,4,4>

59

Ol12:Ol13:Ol14:c

Ol15:Ol16:Ol17:Ol18,Ol19:O120:O121:O122:O123:O124:cO125,O126:O127:O128:O129,O130:O131:O132:O133:O134:O135:O136:O137:O138:O139:O140:O141:O142:O143:O144:O145:O146:O147:O148:

999101106

104105

±⁼ ±+1ifCmod(i,100).eq.O) write{'.101} i,

theneO

call matmul{a,4,4,p,4,

cal1 matmul{dul,4,4,ct call matmul{c,1,4,p,4,

cal1 matmul{du4.1,4,ct cal1

matmuls(du3,4,1,1

write{',106} i.{du6(k,

endi[

callii(eO.

goto

matprn(p,4,4)

lt.epsl)

goto

100

4,du1,4,4)

,4,1,du3,4,1}

4,du4,1,4}

,4,1,du5,1.1) .O1{s+du5{1,1)),

1),k=1,4)

999

'cont-nue

format(i6,el8.1O)

format{i6,4e18.1O)

call matmul{a,4.4,p,4,

cal1 matmul{dul,4,4,ct

call matmul{e,1,4,p,4, cal1 matmul(du4,1,4,ct

call matmuls{du3,4,1,1

write(k.") ⁱ

Esmation

cal1 matprn{du6,4,1)

4,dul,4,4)

,4,1,du3,4,1)

4,du4,1,4)

.4,1,du5.1,1)

.Ol(s+du5(1,1)), ,Gaini

open{10.file=file2)

do 104 ±=1,4

write{10,105} du6Ci,1) iormat(el8.1O)

close(1O>

pauseend

du6,4,1)

dU6r4,1}

Iifstva

OOOI:OO02,OO03:OO04:OO05:OO06:OO07:OO08:OO09:OOIO:OOII;OO12:OO13:OO14:OO15:OO16:OO17:OO18:OO19;

t7-f-7tere ^vts ^kklwaXdi ^V

}-prograrn

implicit

common

cormon

cornmon common

simob

aV-Vltz

real'8

(a-h,

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

dul(4,4),du2(4

du5{4,1},du6{4

x(4,1),xO(4,1)

character'64 filel,

character+1

tab

tab=char(Z'09i}

open{1O read(1O

read{1O

read<1O readCIO read{1O

o-z}

c{1,4),gain{4

,4),du3C4,1),

,1},du7(1,1)

file2,Ei

.file='Init')

,'} [ilel

,t>

file2

,') file3

,t) f±le4

,t} num

le3,

,1).g(1,4)

du4{4,1)

file4

60 O020:O021:O022:O023:O024:O025:O026:O027:O028:O029:O030:O031:OO]2:O033:O034:O035:ee36:O037:cO038:cO039:cO040:cO041:O042:eo43:oe44:oe4s:O046;O047:O048;O049:O050:cO051:cO052:O053:O054:O055;O056:O057:O058:O059:O060:O061:O062:O063:O064:O065:O066:O067:O068:O069:O070:O071:O072:O073:O074:O075:cO076:cO077:cO078:cO079:c

close(10}

write{',")

i _Filel ',fUel

write(',') ^' File2 ^',iUe2

write(',') ⁺

File3

^T.file3

write(t,') ^'

FUe4

^',fUe4

write(t, ^±

)

^' Data

nuirtber',num open(10,file=filel)

read<10,t) w

read(10,')

{(p(

^±,j),j=1,4),i=1,4)

read(10,")

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

read(10,'}

(c(1,

^±

),i=1,4)

close(10) open(10,Eile=fUe2)

read(10,'}

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

close(10)

open(1O,fUe="CGain')

read(10,') gO

read(10,")

{g(1,

^±

},i=1,4}

close(10)

wr ±

te{',*)

^' Matrix P ^'

caU matprn<p,4,4)

writeC+,')

Matrix Q

call rnatprn{q.4,1)

wrke(',+) ^' Matrix c ^"

cal1

matprn{c,1,4)

write{',') ^"

LQ Observer Gain

call matprn(gain,4,1)

write{",") ^'

LQ

Controler

^Gain

call

matprn<g,1,4)

call matzero{x,4,1) call matzero{xO,4,1}

r=loo.o yestO=O.O

pause

^' Hit

return

key to

continue

open(10,file=file3) open(11,file=file4)

surn=O.O

do 100 i=1,num

^read(10,')

y

read(10.') u

call matmul(gain,4,1,c,1,4,dul.4,4)

call matsub{p,4,4,dul,4,4,du2,4,4}

cal1 matmul{du2,4,4,x,4,1,du3,4,1}

call rnatmuls(q,4.1,u,du4.4.1}

call matmuls(gain,4,1,y,du5,4.1) call rnatadd{du3,4,1,du4,4,1,du6,4,1)

call 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)

^xO(1,1)=y

call matmul{p,4,4,xO,4,1,du3,4,1) ca11 matmuls(q,4,1,u,du4,4,1)

61

O080:c

O081:c

O082:c

O083:O084:O085:O086:c

O087:c

O088:O089:O090:O091:O092:O093:O094:O095:O096:O097:O098:O099:OIOO:OIOI:OI02:OI03:

iifstwn

OOOI:+

OO02 ^: ^de OO03:.OO04:k OO05:t

OO06:

^± OO07:+

OO08:t

OO09

:de

OOIO:+

OOII:"

OO12:k OO13:t OO14:t OO15:t oe16:+

OO17:*

OO18:*

OO19:t

eo2o:t O021:t

O022:t

O023 :de

O024:t

O025:k

O026:i O027:+

O028:t O029: ^± O030:t O031: ^de O032: ^de

100

103101102

^call matadd(du3,4,1,du4,4,1,xO,4,1)

^cal1 matrnul(e,1,4,xa,4,1,du7.1,1}

yestO=du7(1,1)

^cal1 matmul{c,1,4,x,4,1,du7,1,1)

yest=du7(1,1)

sum=sum+dabs(y-yest)

write(',le3} dble{i)XIOO.,tab,y,tab,yest,tab,dabs{y-yest) write<",101) i,y,yestO,dabs(y-yest)

if(mod<i.1O).eq.1)

then

write(11,102) dble(i)!100.,tab,y,tab,yest,tab,dabs{y-yest),

& tab,x(1,1),tab,x(2,1),

& tab,x(3,1),tab.x(4,1)

endif

,

contlnue

write<'.')

surnldbleCnum>

close(10) close{11)

format(f5,2.al.e18.10,al,e18,10,al,e18.10}

fermat<i5,eZ8.10,e18.10,e18.10}

format(f5.2,al,el8.10,al,el8.1O,al,e18.10,al,el8.10,

&

al,e18.10,al,e18.10,al.e18.10)

pause

^' Hit return key to end

program

end

NSrk ^I ^-Wt

^1}^op

#. reVfJv ^-f

^tz

^matrix addition

^subroutine matadd(x,tc1,irl,y,ic2,ir2,z, ^matrix

subtraction

^subroutine matsub(x,icl,irl,y,ic2,ir2.z.

^matrix ^multiply

^subroutine

matmul{x,ic1,irl,y,ic2,ir2,z, ^matrzx prlnt

^subroutine matprn(x,icl,irl}

'

matrlx

turn

^subroutine ^mattrn(x, ±cl,irl,y,ic2,ir2}

matrix ±nitial ±ze ^subroutine matidn{x,icl,irl}

'

matnx zero

^subroutine ^matzero(x, ±cl,irl)

'

matrlx copy

^subroutine ^mat ^¢

py(x,icl,irl,y,ic2,ir2)

^matrix ^multiply ^scaler

^subrout ^±ne matmuls(x,icl, ±rl,scal.y,ic2,

^matrix

dia]gnostic absolute sumation

^subroutine absum(x,icl,irl,ret)

matrix inverse

ic3.ir3)

ic3.

r3}

ic3,ir3)

ir2)

62

O033:" ±con=O excute matinv

Oe]4:t icon=-1 not excute mat ±nv

O035:' subrout ±

ne

matinv(a,icl,irl,icon)

O036:

^de

O037:t matrix determ ±nat

O038:t

subroutine

matdet(a,icl,irl,ret)

O039:

^de

O040:t matrix e ±genvalue ^and eigenvector

O041:"

subroutine eigen(a,icl,irl,t,ic2,ir2,number)

O042:+

O043:'

LU

d±cornpos ±

tion

O044:'

subroutine lu(ndim,n,a,x,b)

O045:.

O046:*

O047:O048:'

matrix dialgnostic absolute sumation

t t

O049:de subroutme absum(x,icl,irl,ret)

O050:O051:

subroutine absum(x,icl,irl,ret>

O052: implicit real'8

(a-h,o-z}

O053: dimens ±on x{icl,irO

O054:O055:

sum=O.O

O056: do lOO i=1, ±cl

O057: sum=sum+dabs{x(icl,icl)}

O058: 100 centinue

O059: ret=sum

O060: return

O061:

end

O062:O063:det"dedekdetdetkttttktk"tttttttttttttttttttikkdekt"-tkttkt""tt"detde

O064:' matrix addition

O065:*-++dv*tkktkkkttdeWde-tkk ^±+dek++thk"dedededett*tktt*tk-t+t**dek+*"kkt

O066:O067:

subroutlne matadd(x,icl,irl,y,ic2,ir2,z,ic3,ir3)

O068;O069:

implic ±

t

real'8 (a-h,o-z) O070: dimension x(icl,irl).y{ic2,ir2).z(ic3,ir3)

O071:O072:O073:O074:O075:O076:O077:O078:oe7g:O080:O081:O082:O083:O084:O085:O086:O087:O088:O089:

100

200

&

±f

((icl.ne.ic2).or.(ic2.ne.ic3).

{irl,ne.

±r2).or.(ir2.ne.ir3>.

write{t,de) ^'matadd parameter

write{',100) tcl.irl

write{',100) ic2,ir2 write(t,100)

ic3,ir3

^forrnatC

^±

4,'",i4}

pause ^"

^Hit

^return

^Key'

^stop

end

if

do 200 i=1,irl

do 200

j=1,icl

z(j,i)=x(j,i)+y(j,i)

'contrnue

return

end

or.(ic3.ne.icl}.or.

or.(ir3.ne.irl})

then

error

O090:+t*tt ^±tttdetttit+tttttttttttt+tttttttttkttttktik+tt*-iktikkikkkt

O091;" rnatrix subtraction

O092 :+++k--t de tt-tt-k- de de de t-k de +de de de-de de kde de tk de -ttk+ttk de *ttt"-t+t*+t+w

63

O093:O094:

subroutine

^matsub(x, ±cl,irl,y,ic2,ir2,z, ±c3,ir3)

O095:O096:

implicit

real'8

(a-h,o-z}

O097; dimension x(icl, ±rl).y(ic2,ir2),z(ic3,ir3}

O098:O099:

if {{

^±cl.ne.ic2).or.{ic2.ne.ic3).er.(ic3.ne.icl).or.

OIOO: &

{irl.ne.ir2).or.(ir2,ne.ir3).or.Cir3.ne.irl)) then

OIOI: wr ±

te(",'>

^Fmatsub parameter ^error' OI02: wr ±te(',100) icl,irl

OI03: write(k,100) ic2.ir2

OI04:

write(de,100)

ic3,ir3

OI05: 100 format(i4,'t', ±4)

OI06: pause ' _Hit

_return

_Key'

OI07: step

OI08: end ±f OI09:ollo:

do

2oo ±^=1,irl

Olll: do 200

j=1,icl

Ol12:

z(j,i)=x(j,i}-y(j,i)

Ol13:

200 cont ±nue

Ol14: return

Ol15:

end

Ol16tOl17:"tdetkkttkktktttktdekdektk*ttktdethde

±de+dettkktttttttk+*detdek""tttt

Ol18:' rnatrix multiply

O119:-W*det-++tttkt+ti+tde-*++++-t+deSkide--r-rde-+++t*de*t-t+det+tde**t*+

O120;O121:

subroutine matmul(x,icl,irl,y,ic2, ±r2,z,ic3,ir3}

O122:O123:

implicit real'8 (a-h,o-z)

O124: dirnension x(icl, ±rl),y(ic2,ir2),z{ic3,ir3}

O125:O126:

if(( ±cl.ne.ic3).or,(irl.ne.ic2}.or.Cir2,ne.ir3))

then

O127: write{',') ^'matmul parameter ^error' O128t write(',100) tcl,irl

O129: write{',100)

ic2,ir2

O130:

write(",100) ic3,ir3

O131: 100 forrnat{i4,'",i4)

O132:

pause

Hit

return

Key'

O133:

stop

O134: end

if O135:O136:

do 200

i=l icl

O137: do 200

j=1,ir2

O138: sum=O.OE+O

e139: do 201 k=1

irl

O14O: sum=sum+x(i,k)dey(k,j)

O141t 201 continue

O142:

z{i,j)=sum

O143: 200 continue

O144: return

O145: end

O146:O147:ttt

±de*ktt ±k± ±t**t ±t ±t±**tdedettt*-tt*t ±ttt*ttt ±tttit ± k ±±k ±±rkt

O148:" matrix

print

O149:*ttttktc+ttt ^±t*t**++t+tttttt*tt-tkde"tkttt-ttde*dede"tde*-kt*tttt

O150:O151:

subroutine matprn(x,icl.irl)

O152:

O153:O154:O155:O156:O157:O158:O159:O160:O161:O162:O163:O164:O165:O166:O167:O168:O169:O170:O171:O172:O173:O174:O175,O176:O177:O178:O179;O180:olgl:O182tO183:O184:O185:O186:O187:O188,O189:O190:O191:

O193:O194:O195:O196:O197:O198:O199:0200:0201:0202:0203:0204:0205:0206:0207;0208:0209:0210:0211:0212:

100101102103104106

200105

O192:k+dekdetdetdedet"tkktt"ttdett"-ttttttttttt"kdedek"ttttttttttkt"-dedet

±ttttt t-t derkttt* dettt

100

impl ±cit real"8

(a-h,o-z)

dimension

x(ic1,i=Z)

if ((irl.gt.6).or.(lrl.lt.1))

then

return end ±f

formatC2e15.6)

format(3e15.6) format(4el5.6)

format{5e15,6)

format(6el5.6) format(

elS,6)

do 200 i=1,icl

(irl,eq.1)

then write(',106) x(i,1) end if

^±f

(irl.eq.2) ^then

write('.100)

(xCi,j),j=1,irl)

end if

(

^±rl,eq.3)

^then

write{',101)

(xCi,j}.j=1,

^±^rl)

end

(irl.eq.4) ^then

write{'.102}

(x(i,j),j=1,irl)

end

if

(irl,eq.5) ^then

write('.103)

{x(i,j),j=1,irl) '

end if

^±f (±

rl.eq.6)

then

write(",104)

(x(i,j),j=1,

^±rl}

end if

contlnue

format(f}

writeC+,105) return

end

,matnx

turn

subroutine

mattrn(x,icl,irl,y, ±

c2,

^±r2)

implicit real'8

{a-h,o-z)

dimens ±on x{icl,irl),y{ic2,ir2)

±f

(Cicl.ne.ir2).or.(irl.ne.

^±c2})

^then

^write(t,') ^'mattrn

prarneter

error'

write(de.100)

icl,irl

write{+.100) ic2,ir2

format( ^±

4,''",i4)

pause ^'

^Hit

^return

^Key'

^stop

'end

do 200 i=1,icl do 201

j=1,irl

y(j,i)=x(i,j}

02130214021502160217021802190220022102220223022402250226022702280229023002310232023302340235023602370238023902400241024202430244024502460247024802490250025102520253025402550256025702580259026002610262e263026402650266026702680269027002710272201200

+thtdet"tdett*tkt*+dedettktt+*Stttdetdett**t*dede""de**t+dedetkttdede**tk 'tttt-tttdekttk

±ktkt ±ttttttiktttttt ± ±ttttttttt ±tttttk ±ttXtt ±tt

200

-k dett dedede-de tkkttt*tttk*t+tkttkttktkttttttt dedett dedektttkk -,ttt-de

+de+det+-tk-tdetkdededekde+ktktkttttttde-tttttthttt*-kk"tttktttt*itt+

200

t ±dett ±t ±tt+ ±tde+kde*tt+ttt++-ttt+k+dett+de+ ±ttde++kkk ±+kkttt+de+k

tktdetttht*-tk**ttkdekdek"ktdedetdettt-detkdetdedetttt"detttttdekdetttkktt

leo

contlnue contlnue

return end

matrix ±

nit

alize

subreutine matidn{x,icl,irl)

implicit real"8

(a-h,o-z)

d±mension x(icl,irl>

200

^i=1,trl

do 200

j=1,icl

if (

^±

,eq.j} then

^x(j, ^±

}=1.0

else

^x(j,i)=o,e

end

^iE contlnue

returnend

matnx ^zero

subroutine

matzero(x.icl,irl)

implicit real'8

(a-h,o-z)

dimension

x(icl,irl)

do

200 i=1,irl do 200

j=1,icl

x(j,i)=O.OE+O

contmue

return end

matrlx copy

subroutine matcpy(x, ±cl, ±rl,y,ic2,ir2>

implicit ^real'8

Ca-h,o-z)

dimension

x(Sc1,i=1),y(ic2,ir2)

(<icl.ne.ic2}.or.(irl.ne.ir2))

then ^wr ^±te{+,de) ^`matcpy prameter ^error`

write(",100) icl,irl

write{',10O) ic2,ir2

format(i4.'t',i4)

pause ^'

^H

^±

^t

^return

^Key'

stop

end

if

66 0273:0274:

de 200

i=1,irl

0275: do 200

j=1.icl

0276: y{j,i)=x{j,i>

0277:

200

continue

0278:0279:

return

0280:

end

0281:0282:detdettttstdes++-+i++t+++++++ttk-+tdett*+detttttt-t+dedede"dede-t++-+

0283:" matrix multiply scaler

O284:ttttttk de de de de detth"de tttt+++ktttttt-t"ktde ^de^de^de *t+ de ++++**tttttttt-rt

0285:0286:

subroutine matmuls(x,icl,irl,scal,y,ic2,ir2)

0287:0288:

implicit

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

0289:

dimension

x<icl,irl),y{ic2,ir2}

0290:0291:

if ({icl.ne.ic2).or.{irl,ne,ir2}) ^then

0292: write(t.') ^'matcpy

prameter

error'

0293: write(',100} tcl, ±rl

0294: write{',100) tc2,ir2 0295: 100

format{i4,"t".i4)

0296: pause ^E

^Hit

^return

^Key'

0297: stop

0298: end if 0299,o3oo:

do 2oo i=1,irl

o3ol:

do 2oo

j=1,iel

0302: y<j,i)=scal'x<j,O 0303: 20e continue

0304:0305:

return

0306: end

0307:0308:ttt

±ttt ±-++de+++tk+++t+++-tttdek*++**-+t++t+-thtkt- ±+dekktttitt

0309:+ matrix inverse

0310:t iqon=O excute matinv

0311: ^± icon=-1 not excute matinv

0312:ttttthttt-detkdede+tt--dett+***tttt"ktttdetht"dede*++k-*tttktttdetdekde 031]:0314:

subrout

ne

matinv(a,icl,irl,icon)

0315:0316:

implic ±

t

real'8

{a-h,o-z}

0317:

dimension a(tcl, ±rl>,noseq(100)

0318;0319:

n=icl

0320:

if <n.gt.100) ^then

0321: write{",') ^'matinv matrix dimension exceed lOO."

0322: icon=-1

0323: pause ^:

^Hit

^return

^Key'

0324:

_return

0325; end if

0326:0327:

if ((icl.ne.irl).or.{n.le.O)) ^then

0328: write{",") ^'matinv parameter

^errer'

0329; write{',1) icl,irl

0330:

1 format{i4.''i,i4}

0331: ±con=-1

0332; pause ^'

^Hit

^return

^Key'

0333:0334:0335:0336;0337:0338:0339:0340:0341:0342:0343:0344:0345:0346,'O347:0348:0349;0350:0351:0352:0353:0354:0355:0356:0357:0358:0359:0360:0361:0362:0363:0364:0365:0366:0367:0368:e369:0370:0371:0372:0373:0374:0375:0376:0377:0378,0379,0380:0381;0382:0383:0384:0385;0386:0387:0388,0389:0390:0391:0392:

50 100

7071

=eturn end if'if

(n.eq.1)

^a(1,1)=1.

icon=O

return

end if

thenOE+O1a{1,1)

epsl=1.0E-15

do 10 nn=1,n

noseq{nn)=nn

do 100 nn=1.n p=O.OE+O do 20

i=nn,n

(p.lt.dabs{a(i,

p=dabs(a( ^±^,1)}

^wr ^±

te(+.+}

^' matinv

ip=i

end

'

contmue

if (p.le.epsl)

^then

^write{t,+} ^b ^rnatinv icon=-1

return

end if

nw=noseq(ip)

noseq(ip)=noseq(nn}

noseq{nn)=nw

do 30

j=1,n

^w=aGp,j) a(ip,j}=a(nn,j)

^a{nn,j)=w

1)))

w=a(nn,1)

do 40

j=2,n

a(nn,j-1}=a(nn,j}lw

a{nn,n}=1.0E+O!w

then

return ^i,p

do 50

i=1,n

(i.ne,nn) ^then

^w=a{i,1)

^do ⁶⁰ j=2,n

aO,j-1)=a(i,j)-w+a{nn.j-1) a(i,n)=-w*a{nn,n)

end if

contlnue

'contlnue

do 200 nn=1,n do 70

j=nn,n

(noseq(j)-nn)

70,71,70

'

contlnue

noseq(j)=noseq(nn)

67

0393: do 80 i=1,n

0394: w=a(i,j)

O395: a(i,j)=a( ±,nn}

0396: 80 a(i,nn>=w

0397:

²⁰⁰ ^continue 0398: icon=O 0399:0400:

return

0401: end

0402:0403:***S+ttkdedekdekdettititttttt+t*t"tt+ttt-*tt-t-det+-ttktde"tdede+dede 0404:' matrix determinat

0405:*ttt*tt+++t+tdettttttttt ^±^kttttt ^±ttttt**ttttttttttt+**ttt++tt

e4o6:0407:

subroutine matdet(a,icl,irl,ret}

0408:0409:

implicit

^real'8

<a-h,e-z)

0410: d±rnens ±on a{ ±cl,irl},u{50,50)

0411:0412:

n=icl

e413:

f

{n,gt.50)

^then

0414: write{',t} ^'matdet matrix dimension exceeds 50.' 0415:

pause

^' Hit return Key'

e416:

stop

e417: end ±f 0418;0419:

±f

<(icl.ne.

^±rl}.or.(n.le,O)) then 0420: write(',") ^rmatdet parameter ^error' 0421: write(",1) icl,irl

0422: 1 foTmat<i4,i"'.i4)

0423: pause ^'

^Hit

^return

^Key'

0424: stop

0425: end if 0426:0427:

ret=1.0E+O

0428: do lOO

j=1,n

0429:

pivot=O.OE+O

0430: do

70

i=1,n

0431: 70

pivot=drnaxl(pivot,dabs(a(

^±,j)}>

0432: do 80

i=ln

0433: 80 u{i,j>!=a(i,j)!pivot

e4]4: 100 ret=ret"pivot

e435: write{",'} ^' ^matdet

l',ret

e436;0437:

nmax=n-1

0438: do 500 ts=1,nrnax 0439:

pivot=O.OE+O

0440: do 200 i=is,n

0441: do 200

j=is,n

0442:

if {pivot.lt.dabs(u{

^±,j>}) then

0443: pivot=dabsCu(i,j)}

0444: ±max= ±

0445: jmax=j

0446: end ii

0447: 200 continue

0448:0449:

ret=ret"u(imax,jmax}

0450: write(',") ^' matdet 2',ret

0451:0452;

do 300 i=is,n

69 0453:0454:0455:0456;0457:0458:0459:0460:0461:0462:0463:0464:0465:0466:0467:0468:0469:0470:0471:0472:0473:0474.

0483:0484:0485:0486:0487:0488;0489:0490:0491:0492:0493:0494:0495:0496:0497:0498:0499:0500:0501:0502:0503:0504:0505:0506:0507:0508:0509:0510:0511:0512:

300

310

400 500

w=u{i.jmax}

u{i,jmax)=u(i,is) ^u{i,is)=w

do 310

j=is,n

w=u{imax,j) u(imax,j)=u(is,j}

^u( ^±^s,j)=w in=is+1

pivot=1.0E+OluCis,is}

do 400

i=in,n

do

400

j=in,n

u(i,j)=u{i,j)-pivotku(i, coritinue

ret=reVu(n,

write(-,t} ⁱ

return end

matdet 3',ret

is}'u{ ±s,j)

.t+-

de+++++++t+++++++++ttde detdet-t t*Skktt*ttttt+de det+ dedet+tt de*-***

0475:' matrix eigenva ±ue and eigenvectQr

0476:tt*tttttt ^±^tttt ^±^t ^±^t ^±^tttttt ^±St+ttdedetttt ±t ±ttkttkkt+de*ttttttt ±

0477:0478:

subroutine eigen{a,icl,irl,t, ±c2,ir2,nurnber)

0479:0480;

±mplicit realde8

(a-h.o-z)

0481: dimension

a{icl,irl>,t( ±c2, ±r2)

0482:

'

(icl.ne.

^±r2))

^then

({icl.ne.ic2).er.(irl.ne.ir2}.or.

write(',"} ^'eigen

prameter

^errorb write(de,10) icl,irl

write(',10) ±c2,iT2

format

(i4,'"

,i4)

pause ^"

^Hit ^return

Key'

^stop

end if

n=iclmark=O

left=O iright=1

ep=1.0E-30 eps=1.0E-15

call rnatidn{t,ic2, nml=n-1

⁷⁰ it=Z,100

{mark.gt.O)

number=1-it return end li

ir2)

then

do 100 i=1,nml

aii=a(i.i)

ipl=i+1

do 110

j=ipl,n

^aij=a{i,j)

^aji=a{j, ^±

)

70

0513:0514:0515:0516,0517:0518:0519:0520:0521:0522:0523;OS24:0525:0526:0527:0528:0529;0530:0531:0532:0533:0534:0535;0536:0537:0538,0539:0540:0541:0542:0543:0544:0545:0546:OS47:0548:0549:0550:05Sl:0552:0553:0554:0555:0556:OS57:0558:0559:0560:0561:0562:0563:0564:0565:0566:0567;0568:0569:0570:0571:0572:

120150130110100

140

190210

200180

220

230 250

260270

240280

290

(dabs(a

^±

j+aji}-eps) 120,120,130

{dabs(aij-aji)-eps)

110.110.150

if {dabs(aii-a(j,j))-eps}

110,110,130 goto 140

,

contlnue

,contmue

number= ±

t-1

goto 410

mark=1

do 160 k=1,nml

kpl=k+1

do 160

m=kpl,n

h=O.OE+O g=o.oE+o

hj=O.OE+O

yh=O.OE+O

do 180 i=1,n

^aik=a(i,k}

^a±m=a(i,m) te=aik'aik

tee=airn*aim

yh=yh+te-tee

if {i-k)

190,200,190 ^±f

{i-m)

21o,2eo,21o

^aki=a(k,i}

^ami=a(m,i)

h=h+aki'ami-aik"aim

tep=te+amideami

tem

±

_tee+ak _±^+ak _±

g=g+tep+tem hj=hj-tep+tem

'

contlnue

h=h+h

d=a{k,k}-a(m,m)

^akm=a(k,m)

amk=a{m,k)

c=akm+amk e=akrn-amk

{dabs(c)-ep)

220,220,230

cx=1,OE+O

sx=O.OE+O

goto

240

cot2x=d/c

if (cot2x)

250.260.260 '

slg=-1.0E+O

goto

270

^sig=1.0E+O

cotx=cot2x+(sig'dsqrt{(1.0E+O)+cot2x'cot2x))

'

sx=sigldsqrt{(1.0E+O)+cotx'cotx) cx=sxdecotx

if (yh}

280,290,290

tem=cx

cx=sx sx=-tem cos2x=cxtcx-sxtsx

sin2x=t2.0E+O)'sx+cx

d=d+cos2x+c+sin2x h=h'cos2x-hjtsin2x

den=g+(2.0E+O)'(e'e+d'd)

71

0573:0574:0575:0576:0577:0578:0579:0580:0581:0582:0583;0584:0585:0586:0587:0588:0589:0590:0591:0592:0593:0594:0595:0596:0597:0598:0599:0600:0601:0602:0603:0604:0605:0606:0607:0608:0609:0610:0611:0612:0613;0614:0615:0616:0617:0618:0619:0620:0621:0622:0623:0624:0625:0626:0627:0628:0629:0630:0631:0632:

300

310

320

340350

380

360

400

370160 70

410 '

contLnue number=1OO return end

subrout

^±ne

implicit

dimension dimension

tanhy=(e'd-h12.0E+O}lden

{dab${tanhy}-ep> 300,300,310

chy=1.0E+O

shy=O.OE+O

goto 320

chy={1.0E+O)1dsqrt({1.0E+O)-tanhy'tanhy)

shy=chy'tanhy

cl=

chy+cx-shytsx c2= chydecx+shy'sx sl= chy"sx+shytcx s2=-chyisx+shy+cx

'if

{dabs(sl)-ep}

340,340,350

{dabs(s2}-ep} 160,160,350 mark=O

do 360

i=1,n

^ak ^±

:=a(k,i}

^ami=a(m,i}

^a{k,i)=

cl"aki+sl'ami

^a{m,i)= s2"aki+c2'ami if

{left)

360,360,380 tki=t(k,i)

tmi=t(m,i)

t(k,i)=

cl'tki+sl"tmi t(m,i)= s2'tki+c2'tmi

'

contmue

do 370 i=1,n

^aik=a(i,k}

^aim=a(i,m)

^a{ ^±,k)= c2'aik-s2'aim ^a< ^±,m)=-sl'aik+cl ±aim

tt

^if

{iright)

370,370.400 tik=t(i,k)

tim=t{i,m)

t(i,k>=

c2'tik-s2'tim

tCi,m}=-sl'tik+cl'tim

contlnue

,contlnue

lu(ndim.n,a.x,b}

rea1de8

ifCndim,le

^write{t ^write{'

stop

endif

Ca-h,o-z}

a{ndim,ndim),b{ndim),x w{100),v{100),ip(100)

.O.tde)Jde}

,(ndim}

.ndim.gt.1OO)

_then

LU

parameter ^error ^ndim ⁼ ^', LU decomposition not excute'

call ancornp(ndim,n,a,anorm) write(',2OOO} anorm

call decomp(ndim,n,a,w,ip)

call solve(ndim,n,aib,x, ±P)

ndim

0633: call estcon{nd ±m,n,a,v,ip,w,anorm,cond)

0634:c

write('.2001}

(x{i),i=1,n>

0635: write{'.2002} cond 0636:

0637:

²⁰⁰⁰ ^format(1` --- _DECOMP

_and

_SOLVE ---'1

0638: &

^' 1-norm of

A=

^',IPE13.6)

0639:c 2OO1 format{'

solut

on

X(±

>i1(IX,5F9.5})

0640: 2002 format('

condit

±on number ⁼ ^'.IPE13.6}

0641;

0642: return

0643: end

0644:

0645:

0646: subroutine decomp(ndim,n,a,w,ip)

0647:

0648:

implicit

^realk8 (a-h,o-z)

0649:

dimension a(ndim,n),w{n),ip(n) 0650:

0651: data eps ll.OE-16!

0652:

0653: de 510 k=1,n 0654:

ip<k>=k

0655: 510

continue

0656:

0657: do lO k=1,n

0658: 1=k

0659: al=dabs(a<ip(1).kD

0660: do 520 i=k+l n

0661:

if{dabs{a(ip(

),k)).gt,al) ^then '

0662: 1=1

^O663:

al=dabs(a(ip{1),k})

0664: endif

0665: 520 continue 0666:

0667: if(1.ne.k>

then

0668: lv=ip(k) 0669: ±

p{k)=ip{V

0670: ±p{l)=lv

0671: endi[

0672,

^0673:

ii(dabs(a(ip(k),k)),le.eps) goto 900

0674:

0675: a(ip{k}.k)=1.0E+O/a{ip(k>,k) 0676:

0677: do 30 i=k+1.n

0678: a{ip(i},k)=a(ipCi},k)"a(ip(k),k) 0679; do 540

j=k+1,n

0680: w(j)=aap(i),j}-a( ±p{i),k)'a(ip(k),j)

0681: 540 continue

0682: do 550

j=k+1,n

0683: a(ip{i),j}=w(j)

0684: 550 continue

0685: 30 continue

0686:

0687: 10 continue 0688: return

0689:

0690: 900 continue

0691: write(',2000) k

^0692: ²⁰⁰⁰ ^format(' (DECOMP) ^matrix ^singular {at',I4,b-thpivot)'}

0693:0694:0695:0696:0697:0698:0699:0700:0701:0702:0703t0704:0705;0706:0707:0708:0709:0710:0711:0712:0713:0714:0715:0716:0717:0718:0719:0720:0721:0722:0723:0724:0725:0726:0727:0728:0729:0730:0731:0732:0733:0734:0735:0736:0737:0738:0739:0740:0741:0742:0743:0744:0745,0746:0747:0748:0749:0750:0751:0752:

520 10

540 30

520 10

520

n=-kstopend

subroutine solve(ndim,n,a,b,x,ip}

implicit

^realt8

(a-h,o-z}

dimension a{ndirn,n},b(n),x(n},ip(n)

¹⁰

i=1,n

t=b(ip(i>)

do 520

j=1,i-1

t=t-a(ip(i),j)'x(j)

contlnue ^X(i}iit

'contmue

do 30 i=n,1,-1

^t=x(i}

⁵⁴⁰ j=i+1,n

^t=t-a(

^±p

O),j}kx{j) '

contlnue

x{i)=ttaap( ±

),O 'contlnue

return end

subroutine ancomp(ndim,n,a,anorm)

implic ±

t

real"8

{a-h,o-z)

dimension a(ndim,n)

anorm=O.OE+O

do 10 k=1,n

s=O.OE+O

⁵²⁰

i=1,n

s=s+dabs(a{i,k)) contlnue

f(s.gt,anorm} anorm=s

'contlnue

return end

subroutine estcon(ndim,n,a,v,ip,y,anorm,cond}

implicit real'8

(a-h,o-z}

dimension

a(ndirn,n),v(n},y(n),ip{n>

do 10 k=1,n

t=O.OE+O

do 520 ±^=1,k-1

t=t-aOp{i),k)"v{i>

contlnue

s=1.0E+O

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

OOOI:OO02;OO03:+

M- lftjet7vs-7t

(a-h,o-z)

du7(4,4),du8(4,4},du9{4,4)

lel,file2

fUe2

{{a(i,j).j=1,4},i=1,4)

open(10,iile='Weight')

Weight

cal1

mattrn(a,4,4,at,4,4)

58

O052;c

O055:c

O058:c

O061:O062:c

O064:c

O068:c

O074:O075:O076:O077:O078:O079:c

O089:c

O092:c

100

j=1,4

ddd=a(i.j)

j,i,a{i,j),i,j

writeC',') j,i.at(j,i),i,j

'

,

contlnue

Matrix Pl'

Q'

Matnx

Matrix At'

call

matprn{ct.4,1}

pause

i=ocontlnue

cal1 matprn{dul,4,4}

cal1

Matr

Matrix

Mat=ix

Matrix

cal1

7'

matprn{du7,4,4)

cal1

' Matr

Matrix

call matadd{q,4,4,du9,4,4,dulO,4,4)

Matrix

cal1 absurn(pl,4,4,eO)

59

cal1 matmul{du4.1,4,ct cal1

endi[

goto

4,du1,4,4)

,4,1,du5,1.1) .O1{s+du5{1,1)),

'cont-nue

format(i6,el8.1O)

cal1 matmul{dul,4,4,ct

Esmation

.Ol(s+du5(1,1)), ,Gaini

open{10.file=file2)

close(1O>

Iifstva

t7-f-7tere vts kklwaXdi V

}-prograrn

cormon

(a-h,

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

dul(4,4),du2(4

tab

read{1O

o-z}

file2

60

i Filel ',fUel

File3

^writeC',') j,i.at(j,i),i,j

^7'

' _Matr

t7-f-7tere ^vts ^kklwaXdi ^V

i _Filel ',fUel

^Gain

^cal1 matrnul(e,1,4,xa,4,1,du7.1,1}

NSrk ^I ^-Wt

#. reVfJv ^-f

^subroutine