From 79e207c7e075d87108219572d0bf23b442812139 Mon Sep 17 00:00:00 2001
From: ChouHor <ichenqiuhao@icloud.com>
Date: Sun, 20 Feb 2022 19:29:02 +0800
Subject: [PATCH 1/2] 1

---
 __pycache__/control_math.cpython-38.pyc | Bin 17200 -> 17659 bytes
 control_math.py                         |  55 ++++++++++++++++++++--
 fft_conv.py                             |   7 ++-
 fft_factorization.py                    |  40 ++++++++++++++++
 sys_id_fft.py                           |  60 ++++++++++++------------
 test_file.py                            |   4 +-
 6 files changed, 128 insertions(+), 38 deletions(-)
 create mode 100644 fft_factorization.py

diff --git a/__pycache__/control_math.cpython-38.pyc b/__pycache__/control_math.cpython-38.pyc
index 78044b02c27e3bcc74228e5e0b477bdf72866141..d970bccaf5e6e0c145f7b2803d85f7ad8165b4e6 100644
GIT binary patch
delta 6750
zcmbVR3wT^db-r`=?nArMTG9J0$C4$l?Z}byEOBf(e#Kvc<Hm8~T%fnBox9SN_r=^R
zS$5Z~NlZx6kB~5Yd=LnqTpmRMTtFWLC=b(*(54Sc+VZ+hpdXZiX?e7?X_L^$`RA^*
z(&B{BmG6JgoH=vm%$YN1X0AVWAA9iumOR?j6xZnYz);KN__0TleQe;7Ek`R%;~J0T
zF3Y!=E%{cnm1zew9_6vK8jsm+_lC@NDmU;rmE)@1LFEK*q;jJwck(8lJgb>qd>wD5
zcQ?O`x6r$XxAHc6_wsh$LGM1^$-C&?&%1dKy$5(N?>l>&W)AXxK0u81e2}lF_vQR@
zzJcBw_z)ka_YmL6N9aAw$M`0mI?K$Bd>h})w-7bLujE_#6-144c0@~!%bVD~M&>s-
z1<#%mY0qA|k3Fb2x6!~wJD>y5B-b0gO?@Ey2^KXmB5yWc%@Xn*#uu*|CRQpebX11{
zXzfSSu4T`cgi@1=iXkw=MQ_>`Vp!fE9!rcsR>7$JQn;55%V)!1XU%dca?QbR)U^Ow
z0X=|Tz#yOz0LvDelwPqJ4A>#I0JZ|I0F24;=+3THJrUE7T2`8K%i1cfhEaJw`rgrP
zki8PH9q_C2?(C#xU8Ce5jlBZ-{A|v$c9Q&)TV7sw!m>OuYjbDLvJS}G;-9XlVK6cy
z)llbCk%Dn*T57I|63;itKDX-RdOtKlfR%i9ynbAvQ>EjFsn$&rXe`EB=uh64_=>%b
z%G5o2@gv;M*`Do3oPui$Ppl`VxEwVhUi8FDREQ14OX<NxQ$UqwJ<D~b>N|}I+btDI
z)!p*rjrs9S(6%<*7FjT<b+ZtrhPIbkx~=H}mVm_qP7>?n_mjnLWvO0rL??J%HKp?S
zx}l00Bmb0gMb9<kq{Euer6=uNK%Wfgm}}>qTybh1vL(;)ayEH&x_Bbh7>tNzeFG{e
zhV%ARx@NP_9AD3#Fpa6SQ!vA&>9lM6p{z6I8|iu5rSVe;2VNK@yL@A^$nC6L-z;uX
zgCB>n*8r{sTnD%wumf-tU?1Rj05zW|3JTR~h|6c2`&pxWvAI}*Z;-k|^tfATS$dks
z7W6VZ$7VvDwQBdW(~)xIe$6tL`3vDPakBb3M%;+v5)CfbtK6vKhQQ_eifl}A4RG(*
zW@0l9JakvLMr4#oqg$&sj+d$0N|10PsWhtGCX|<(b!#j9WR+jlQRT~tmd=f&rNv`2
ztqW}!&O$r2ZYf8*G|=V1{EqwOJ6lfnl4Mso&b4ynO=NC4)=7!`O*9y?PHt`Oj#A6+
zE~0O3Jy0Rp-g4uz?24Rb^}SikG9Ka4v$4}n<tCEr=NjeB*r6YibLsaFyTpz3s)iyc
zcs~QzFaMAd4SJBsT%?z>emYcU3p6QYgMKgL6?RTPjY4R_z_7q3ihSLvoi@tG5p7XF
zB-EM=`%zj3)9I3}g4|~}`E0kCAg!se&pcC~J7$LW7e&S<@~Elvd9jx&SA}0VPvMt4
zPfRyPpfQ78+rqlosnH`I<^Ln4bjE%)M8+)3DL9^GjqS)4GwGbWb4)c&4frA2E<}s;
zTl)rt4fRuiEP>DV_+e$A&-RKA+157EY({cUA)TZ3P;v@$VnSxxZoS%%x^AE@8Lajd
z6`}xTC4ky|@}Oo3OzF2>J>a0If?o|xtpKc}H`C+pm!s|dY`wgtebngC8zWM*Z%rns
z29vM{5}%MCX@4&9p|Z|n$MuJq*egHUvG>lxF=8|*#(TW>g^sc|(@7!K#pCDL>3BK5
z&`lhJ8|U<e9-=7Rm`2?8a(t$j%6*+x&P+ee7p;SZNSW4z{7FY|TjCJqL=_{LKyik^
zlzpA!YMJ_Qjhz;Lh&*mac(Ej$Yf=NkgQ&{!$3ZC{3s7@_3BWv{<|>zpexsXlN+-uC
z>yp348zFHI;9kI4z-ho60F_g~GT`?B4FEV%R0zx@^5pWVpeW4Cg0kZ|>6|r}&du6p
zBUo9hlosi{E2hyD6Xvs%sYEqr&iawO?M)YXFd{XCs#%owYId2CSu1a+3qBL8Qk<Gw
zB*1Ud<L;D$UEAmw7N<?BgPsUWhUDR{_h*%#2GIQ!0AUz7rnVPMfQN8j-XoQ@@L7Eu
z6Fe1O3X}=(+%M&^?!qpFP%0u2Z}kmp0b5&eH5L1)MVQHO&+ySR&=D0Dbo?$L4Nxjo
zvRCs2`Bi4r6irgaee|fs8)LD<@r|(tF)82B^I^6}Ztfj8T5|`Y$Zt6ASjAb7c4H!W
zm89d^5PEnk;9<ZcfVTluU$qWlSe(hndhgj^9leB9t&xh=w}S_5)dPTd5R5ui7+x!r
zcHhy8;E*+fVb@vlQvU0v{od87oyJZz96q>rOdSXg(R{q5FJH=y9pWKU(*T7LBFK*o
zv50s`-roNQBN<{`#QrI61|$Jkx)(zPG(6?s!?I_fZwmf0qdc9b#5U!J>HO>z#SLh1
zH=u4@YrWwoOU0aRm2#AC^LD}OhcV663>n#AjV#IfSTjB1UGlMk4~!@~_EUBBToxRR
zS3=wmWs0siET;!YA}Vc8%eN11YOm`v#IxQeCZ8VcVCUqI27kYT4o{-fRlf@qo$3Uo
zM*4cH5eES1%V)MYOz%{f&PgI;@w8Vx3cUs*7`v%kg}RydW7MzZ=87k_W0W|)<G7h*
zu|^u`)l9rkzOepN%9z?t`aGoQcSfP6U05EtN{Q+oP^2n9LiwjKWve9Vhj8V9fntIH
zd*Xa>9bpA!oX_(0wk2WPoSpIdi^4f&^BY!7)RUBK;%Xwq<4d=0=w}J|D=;xBy`jw&
z1crD&x@ugNuWQD|??kHHCqz+*$4ShOWviYLA4F4S>IYDf$P^11+JFo6h#6=Mi$8=&
z8)_r-mOW<|{P4V0<lK*B(;1Icwym9Cah_!N$?@Th?EKPQ!|!HwE)^e!!jBLfQbq^+
z5+Z9k2F?&ozn_0GweFJRG3bcAU3`>8{7~L02$;TVBoipw0F1zoyl&3*(wXB}8gq6o
z6|arRj233|7F2u);%d<>5@m+-X*!^bkAeGfz$XA}$LSjuxzUtg9ofR3kPmOVL3VCx
zlLtq)PGKyIk-&5uxPm}_6x52V7elMeNKB{mdFzBz;KdWa(hKV55e`TP8ihiPjj)(5
zK1Y4_%jZWAA3(BD*&)aipGEPH0d)p9qzj&7LEjtK>e~!`pC^6Jfls5|XI2!xw65uN
zE~|7sxK`JQ9%JGQaw7G*t5mk2TI^P8En_~ETYMv9J2|mLWj|C(b4tQNn8~lD+D6uz
zEiRChL?<yKuw=Vf8xt2~eDiB7DuaiW$f|Uq0^$i0rX-^Ds$}#h#1LO1pqr9whH+@%
zW+Zz8?|N5-XlfrtpUY_?hGeLdo4!m6G6n&DOF!8B4C_^iNi9mX5?6-3$V~an)?HgL
zo<-f)$F3BY*xCK~j-2CqnPR@Q^UHGo6?YH)Dai*0{n-kod@_pSpA$SO@7}(rf~`==
zE$ZSa&}yu8-h=p47)gYPh^A{XEp=HCn<^O9#HjqL()a>N`G%8EPl-PwvYOCnIr!`j
zpB?lgUJ=#`3dTD$zbPw<dCQ?>k)F)i0*74O4d!g&+NoA?7c?Lz`}$<T*VFmnvjCDw
zaB_h=eB*Z8+U*+$^VV*WmERg0tKTq;tmAF}pzPhYi+y+LHQS<k&y&zl=XM&SCcYxe
z<GWsg1Ai*NGk)w9$o*`sqs3pKlS_E`7j+(PeR&Uyzl5F@2aCU=qHk~~J>|#fFz(|1
zBfbimzb2@NuYn|&YvO4D{4D;4Ak|j$vG`kX{|+z=SnFl+b@0AHU^bAyPuR}Xv=<yx
zzloY}0lp3J0e=s82Joz=^=jffAlHr?pBKER<tMM&$G*4p;#IHG)n!7|*1;`kbBN#~
z61yLwtgTKn7f(^~4snn$)e~H=)F!W1?F@0ny8$XXFMv`xR7Ix>R-E8y!!4EU0vE?2
zpgc9G_y$h!YLSYgN{+sP%nF-sdcnsR+QeMCN7~{T8r}{#0=NTkr+n_(%M;TeRdt8l
zaNT(FK`Q$!9jx8mGJRcZSUf;HdE&a?-iYX~cT7Ap?TC`)aJ%pV;^MWkuzN^0Uw<Rh
zmkwS3m|pol8vP?c1?>+&sjN|Rp1Q29cu@QkM1Bak8So>5st5aja}QR64>}j2X_fy_
z{4oI8zz@?ckS^Ho0RuTtxl{ZzsDA<c1aL_wQblS|{1o*+1N<DY%Bk9ZD*r2({|5L4
z;NJoN0f4u~a{!eqcZ2#*z<&YYckw*n1x;?-(e+y|60MT>?XxGvf7c4LD({H@g{Vee
z7Iax&Gg0!)j`qm4bdq;xs?(E@PwkjshCFnA|I+A<_q@zBsmWQHD>{y^LpK0FMkgpb
zZjk5RLd6vG*DW)G3#%($$)t8;QY+Ql_iwT;gql<w{g6(w*<eEdfAd$NF<p8Bsp8kq
zpU>vRtKgTl^Jq5AS(#ZO=(b@tl$Oag6H9F{t$V~T(HxU1{s&MunU`96$=cGhN=0{G
zI=g4Dz!fXS_E-OH^22oME1gssnm)gtBzi1U6t?=%ZOX(=H&l>1=+lDwkP&=0QWtkK
ztXS$Z1^ssgZF^p^m~(^7oS-&-q&PW4r}*GM2PR?kLx6Vy&H>&BcpPvZ@C4v<fX@TI
z0Jxyh$bvQZU6j8E&=I}{0Haxzaa2;q!N8C0pl?~TIeRAwXztYnIJET9gTLlbbGf;q
K8M1lL`u_tk*tn_y

delta 6160
zcmbVQ36NYx8GilddNZ^4y~%Ep&1MrO8#V`EkpKzFCWL?lLlR8}@YT!q+ucd#vfu1(
z5(bx3rKE(=pjFC|02Zzw2<uU0RE`2fF7W`hQADN0paNbfLV^F+Z?cme!l64m-|PPS
z@4x@6|L!+$T*1EnDOR(!x;kdi-*>*h*ZJ|LeKj4dVPEgo@l1WPo*C;69_EqD3?6YC
zE)66bsc-Qp^`pAqMEw|#Q$Mcz&Af_NUuGm*cnz<mXDhGc_4I7x4ZM+_?YxOM({l!I
z;jQ$X$=i54Jv;aeK9ioEyn}bra~AKP#b;AvHlM@0=sAba<=yn`;`4Y9J?HXc`2v2-
zWh~jv`*?ycq}DvXnD_EU)av1EvyoULH?nn;``Hy{T_f2ingGp!D!Dv3vw9|)9Rw4G
z=#ftFEEbb@1@Aqsn@EX}FfkkgV77`lp6%|=3oRxQ5pzKd<qE^D5Z&@XsIRUEyc*_H
zE7>BS54W&x`C8~{Rx2M5pRl0~4E2BpKs#UtU=|<_fO-?vTDVAn0A<8NKrdhs;8=M^
zWM$j*<y6GB9nNjLSMHBIw4e`sivddk@8tDHcG&ju#JfOFS~tEwe`X|Y+ox#$o1!=N
zzPJ3iZ5PCd%TuGaeHzVamE*Ak<ch9%^L8DIW1y6dN7)?_qNyP1<(0f!X?qnISYEMR
zmG%Vo=69VfuZln8)(}r3s-3LD+)cX$SA|nq&lLqR8;j0?kO0pW#8mi27bcm0M5C!}
zexzV~siDe6Ea7_j9EsW{AFawPT>xp*^J;?mfvfO#h$gy)9h1Y=SFm_BrZGT`e6uFk
z4*QjvPl*=LS^>ULd0B1uxU$F}`E;(}sTj$ycc+~lZrX3}(e0$=xtUZtH?$kP`9i9Y
zcFBvwxv@l*ZxI_*L5QGa&A3BOS!cyk${ZY1!67G=RiXT_<GCs@m>N<+XSeH-{iKQS
z1thcz?#OZ1mCNfyTpPax${sK8u4_E=Bs5n8)&d3qWiRL!MCcrkQo}>+EoQY_mKB})
z#H@gb%clBV4ZMV+)DPYd$$j<htwqBQ7TA8X$PTdmp<=ig9yAZIqP)Mpd7Q=~dMto3
zZkEOj+tSpao?+~?c1C%i-6$G6Bh(7E8(M;4$BH465GUTMVweX@)N0+Inl(kQDbY(Z
zh<+XvG3;6zs4IpKunQxF`XVcanv6?9Z`eDpz;-t7Z_=ta)A9|)NSi@QH5dJvEgv(o
zEzH2DXks4CYT0gN&22_eUe~aP%&cu}?;>h@F~*Hz_;NClK31!7xjATRliJ&ecSGZP
z;@#KSyo-1{xItAgyzxbHBK<wTN}NoOQu3h%FDE)xAeAlnJBDhUBZRYu5-7FTRaVya
z3QocGRD@D^*vY$!tyXM}V*M&q>aP{e@~x&*YgIU%$~x&GNE;Qa<i_R?_a;h(EQ`f*
zfIL6|bRABkHAbN1lg-Dsmor8u3zF<p^z}}cb6PqhCY#5?tWK_MSs+s_i)xS`A^PeZ
z*(Pskc`d%NXmV?pc{LMj<gKl1<(jtErszi6+S+i`25~8Yl9{%p<FH<9?K&dSEJiV^
ztI<xhGJpbLH((Xu0zlb>5$dWcZy=T5(?|7!tQVJq13HUO13m+|2yijrLcloSlLkT)
zD{asb70$SY;T-ps(5oIrQZhVZXIv+H#Lm)w!oK@DeZ6J!iT1_j3?^eUZXMUAbYigz
z_m>NeHp(KF!^at;s(q9h=Yv)e9GLb5eLZ;9U@-~}K6Pw9+uORi;K;=@H?LYlLy54^
z)uKX+auOhZmC{i~o-yS8Gv{qZz(_w1E*CHe&?0rg(LUByr>voFY2OSH^X2sQU?vru
zg|SX6Du(2Wj@#K9`G<~K@v@&rWo>6iAI^2L4{!}Y0<Hy=#cRpX;8{a%@4V!+l9|_m
zt2{|3)aOBiw9@$^t^}b%6zq7xkX^I3E}t+r3V1XT^i8Z#<lTJrk|FPy)wcd!o%F8$
zxti8e^!367<e#V&7FWyo?3??CiEs#eOsofBw`k1{1*k>VXuq9}c06hFHOc4>C(FlD
zyfCanPMQuAajkrHcGL0|n0Owb!j|byP&N5n+O_j(XTZ(4*}}_)JY`NByIy{D&X?!w
z#n)ji|7iE`Pg9Ak0s}cuoGrV$=7)7%xk{eZb<CoQ1p<7q@H)wu%GegxMxO@yh+E}>
zuFujf?bx}kIei9e>pW<E8URZv<_c|Zh|XLwU~{KXI!r$f7TBpwyK;9bi<Y&$bZ#ub
zXl>kK3}&$!Rz;uVnYfKsZjwKkd-HfLq!5qqW@w7Pi!>U>eTPlg)k$9%KS4t(><A}2
z<m!5*0=S?;M_sQHA;ot4msghc^<$Ze+*37BZ`2(qL~?>^KChCmbT_j*q19yjyeNy8
z)vA`q_AKPcPVp52x?gB7RVWnusFNOX*9nmm;!bF24VD}rzKW?@y$KA&2Xff~I$pB$
z6)8vziQB=_C=d3`Y?>ZEaSx5O$d`J0*gcbV^S{J;%8F@~?!ruz!HrrwzvNfTrxvvO
zrLaQHAYGZ3iMxqk1v06ufJR3ss7s?h1E81+kJ3RnuuFx6GwP<PaQk}FP%@Go$=Hx^
z2PEih{~G9_j6?S|@eRZ1GQ>AA_AS74t5nb?@AS$~AJfYY$_)!ol20X?q?uT>87V&z
z_C4Hyi8?KBKx@i_6M-Z2hz~oNj6Igh^4!=vR>9Z?d2ynV-AAi*%6k$YS&t2_ceKB^
zzmM*{fC?3(PPUM;A>kZkaEw?`?=7~FS*Ez3<fYbs7xTUc(6M-TS;J0xP|G@Zx~%!8
z#l!>h^@Zo_b6M{LIytn_h=6v43J$obwD>mlRUq$hDw0!iP&y7oRH5BAJu0ja57H!#
zHE|GN2ez>$rl|uaD5bxMA)oF&W7!v|OWBA?z79Imb&edwP{dzY7n_H{^pM=NsC8k5
z=K>VlU0Cz*uD58<*O=tBM<%aX^hY*F*A|_}+HJbb5x+6Pa0O3!a`kb`<l}u-F9c7R
z%IsSrj&c~Q*vaXXR~X1;@++^CZ})BQejGY!ZJwBF>F#`Ve?;(kdBNiTahzP5Z^9H$
zqOJWo-3-nCMVJvLj<8`GmXWA0*MHwZx>o9nd<$`@U@GGbiAjjepdX;!px740HmGnR
z2W>^vFt!+~dQjvtc8V&1vm@;aWEXCmqptAWM59PT0_v+WcVw06Wc-)HMv$`rxI+cc
zaqZPAxFKV&7CE_ZNni7H6I5_8RaiDAf3{>5dttI^X~dlILrkvlKU^;!l_wmx>H|3N
zUipRNwtWD;`=&cuJcUK3tEWBuV1<Vp-`m6D$B;ASVDS^`svu7}L&~C?xR<AMRQwb?
zKO-0yPoqgLH^egl_*wj%AkkFzvG@h3zXWsxrh8c^(4HkoM#<k}Zfa<_;9u*1g^^za
zo&)>_@I2tRfZrL$Ohf!0&FOaIWwUUz%$~50y*T-e6IPi1-Y;*6Gce~Yf<t=<RDf!>
zzKtB(OWiHvbi%|8|5c*AK~qE(Hv)8_)O%Q0P@SE+qO7J-Iyv)sH_OFNuxd}usslZ!
z@7Rxkx<y`iVrTQYXl??W1K14MF7G<AD}EK4dbn9Wd*af0S5jXw$KRu;q7!xOfe|6-
zLPM7nZ&a~8a@q1tJ;>-vXT%1EQzCDtxSM?pxp1CL^moh0mY>3`$>@q3%<(^B(jfp1
z8{#E^E*WKq>Gzx|uZcf_<<9_phx-e)N?z;y-@P{Ozu><NaYy(J-B$n}0sV>~@HTl9
z2)+x^dKK_jz~2BzbtnxaW{baL{2zdS0*-L7v55Mwf%q@LzX7-;iT?oLdGQ87*T+-k
zb89->WOP8}JkO>RIvF@;WDj6OF!Ek9JUC_(Mc`6a>Wt_*BSJ(GVL${>k+F5<AlX`Y
zL~dH?(GQ2w6`hm6TY1TQL`lbHDpYiX)-@lXEV>EN1%n*-IqD{usy$4wM{%3-#0QCK
zJ7PLsdWoHYFFxQ_5IUx_pb1rw!9hQs|6lmVY1yO4j|Sd9e2R^V<xt%ZBU*0~B%`B;
z_fayMKim{=Vf<|X{c=Lvf}mfRXd<p62&jnX(RN<V5kbx8ybW?*|43UcO-``WO8@g!
zh_WcZN7T!^`<s}O5BHxmzJ*pk`~!gh>xZyFisCpc<Z@}xuedP~!?_(h>6-2T%!}7j
zaW&w2z^#DW0CxiJ0UQL}2e=>b06^C=9hVoVd!~31L<?5N08sgegw?5w17BGuXL5Wb
c?XDyS!&^dtn@ER#2dN9x73;RjXV=d8FI{{({Qv*}

diff --git a/control_math.py b/control_math.py
index 4d776d2..ecf9788 100644
--- a/control_math.py
+++ b/control_math.py
@@ -3,6 +3,7 @@
 from sympy import simplify, cancel, Poly, fraction, solve
 import matplotlib.pyplot as plt
 from scipy.linalg import expm
+from scipy import integrate
 from itertools import chain, zip_longest
 
 
@@ -145,10 +146,7 @@ def __init__(self, A, B, C, D, dt):
 
         identity = np.identity(n_states)
         self.expM_zoh_x = expm(A * dt)
-        try:
-            self.expM_zoh_u = np.linalg.inv(A).dot(self.expM_zoh_x - identity).dot(B)
-        except:
-            self.expM_zoh_u = B * dt
+        self.expM_zoh_u = np.linalg.pinv(A).dot(self.expM_zoh_x - identity).dot(B)
         n_inputs = self.B.shape[1]
         M_linear = np.block(
             [
@@ -173,6 +171,47 @@ def __init__(self, A, B, C, D, dt):
         self.Ad_step = expM_step[:n_states, :n_states]
         self.Bd1_step = expM_step[:n_states, n_states:]
 
+        def model(t, vX, Force):
+            vU = Force
+            vX = vX.reshape(len(self.B), 1)
+            dx = self.A.dot(vX) + self.B.dot(vU)
+            return dx
+
+        self.ssmodel = model
+
+    # def __init__(self, mass, damping_ratio, stiffness, R, L, kf, fs_pos, fs_current):
+    #     self.mass = mass
+    #     self.damping_ratio = damping_ratio
+    #     self.stiffness = stiffness
+    #     self.R = R
+    #     self.L = L
+    #     self.kf = kf
+    #     self.A = np.mat(
+    #         [[0, 1], [-self.stiffness / self.mass, -self.damping_ratio / self.mass]]
+    #     )
+    #     self.B = np.mat([[0], [1 / self.mass]])
+    #     self.C = np.mat([1, 0])
+    #     self.D = np.mat([0])
+    #
+    #     def model(t, vX, Force):
+    #         vU = Force
+    #         vX = vX.reshape(len(self.B), 1)
+    #         dx = self.A.dot(vX) + self.B.dot(vU)
+    #         return dx
+    #
+    #     self.ssmodel = model
+    #     self.Xstates = np.zeros((self.A.shape[1], 1))
+    #     self.T = 1 / fs_current
+    #
+    #
+    # def run(self, t1, force):
+    #     r = integrate.ode(self.ssmodel).set_integrator("dopri5")
+    #     r.set_initial_value(self.Xstates, t1).set_f_params(force)
+    #     r.integrate(r.t + self.T)
+    #     self.Xstates = r.y
+    #     self.pos = self.C.dot(self.Xstates)
+    #     return self.pos
+
     def response(self, u, method="zoh"):
         last_u = self.last_u
         u = np.asarray(u).reshape((-1, 1))
@@ -198,6 +237,14 @@ def response(self, u, method="zoh"):
                 + np.dot(self.Bd1_linear, u)
             )
             self.y_output = self.C.dot(x_state) + self.D.dot(u)
+        elif method == "ode":
+            r = integrate.ode(self.ssmodel).set_integrator("dopri5")
+            r.set_initial_value(self.x_state, 0).set_f_params(self.last_u)
+            r.integrate(r.t + self.dt)
+            x_state = r.y
+            self.y_output = self.C.dot(x_state)
+            # return self.y_output, self.x_state
+
         self.last_u = u
         self.x_state = x_state
         return self.y_output, x_state
diff --git a/fft_conv.py b/fft_conv.py
index 245024d..4ea2bb7 100644
--- a/fft_conv.py
+++ b/fft_conv.py
@@ -1,10 +1,13 @@
 import numpy as np
 import matplotlib.pyplot as plt
+import scipy.signal
 from scipy.fftpack import fft, ifft, fftshift, fftfreq
 import time
+import numpy
 
-a = np.random.random(200000)
-b = np.random.random(200000)
+scipy.signal.tf2sos()
+a = np.random.random(3)
+b = np.random.random(3)
 T1 = time.time()
 c = np.correlate(a, b, "full")
 T2 = time.time()
diff --git a/fft_factorization.py b/fft_factorization.py
new file mode 100644
index 0000000..93b5c9b
--- /dev/null
+++ b/fft_factorization.py
@@ -0,0 +1,40 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.fftpack import fft, ifft, fftshift, fftfreq
+import time
+
+a = np.array([1, 3, 2, 4])
+b = np.array([5, 3, 2, 4])
+b = a
+a0 = np.array([5, 5, 5, 5])
+b0 = np.array([1, 1, 1, 1])
+c = np.correlate(a, b, "full")
+c0 = np.correlate(a0, b0, "full")
+
+cc = np.correlate(a, a, "full")
+aa = ifft(np.sqrt((fft(fftshift(cc)))))
+np.correlate(aa, np.flip(aa), "full")
+np.correlate(aa, aa, "full")
+num = len(a)
+left_pad = round((num - 1) / 2 + 0.1)
+right_pad = num - 1 - left_pad
+a1 = np.pad(a, (left_pad, right_pad), "constant", constant_values=0)
+b1 = np.pad(np.flip(b), (left_pad, right_pad), "constant", constant_values=0)
+ahat = fft(a1)
+bhat = fft(b1)
+chat = ahat * ahat
+
+a01 = np.pad(a0, (left_pad, right_pad), "constant", constant_values=0)
+b01 = np.pad(np.flip(b0), (left_pad, right_pad), "constant", constant_values=0)
+a0hat = fft(a01)
+b0hat = fft(b01)
+c0hat = a0hat * a0hat
+
+c1 = np.real(ifft(chat))
+c1 = fftshift(c1)
+if num % 2 == 1:
+    c1 = np.roll(c1, 1)
+print(c)
+print(c1)
+print(np.allclose(c, c1))
+print(c0)
diff --git a/sys_id_fft.py b/sys_id_fft.py
index 6831325..8a9beac 100644
--- a/sys_id_fft.py
+++ b/sys_id_fft.py
@@ -8,11 +8,11 @@
 k = 0
 b = 0
 
-# A = np.array([[0, 1], [-k / m, -b / m]])
-# B = np.array([[0], [1 / m]])
-# C = np.array([1, 0])
-# D = np.array([0])
-# rigid_plant_num, rigid_plant_den = ss2tf(A, B, C, D)
+A = np.array([[0, 1], [-k / m, -b / m]])
+B = np.array([[0], [1 / m]])
+C = np.array([1, 0])
+D = np.array([0])
+rigid_plant_num, rigid_plant_den = ss2tf(A, B, C, D)
 #
 res_freq = 200
 anti_res_freq = 150
@@ -28,12 +28,12 @@
 )
 res_den = np.array([1, 2 * beta2 * res_omega, res_omega ** 2, 0, 0])
 
-# num = np.convolve(res_num, rigid_plant_num.flatten())
-# den = np.convolve(res_den, rigid_plant_den.flatten())
+num = np.convolve(res_num, rigid_plant_num.flatten())
+den = np.convolve(res_den, rigid_plant_den.flatten())
 
-num = res_num
-den = res_den
-A, B, C, D = tf2ss(num, den)
+# num = res_num
+# den = res_den
+A, B, C, D = tf2ss(res_num, res_den)
 ss = StateSpaceModel(A, B, C, D, DT)
 
 """Chirp参数"""
@@ -57,7 +57,7 @@
 y = np.zeros_like(u)
 for i in range(len(u)):
     input_sig = u[i]
-    y_output, x_state = ss.response(input_sig, method="zoh2")
+    y_output, x_state = ss.response(input_sig, method="zoh")
     y[i] = y_output
 p = y
 y = np.diff(y, 2) / DT / DT
@@ -95,7 +95,7 @@
     * (1 - np.e ** (-1j * 2 * pi * f_u * DT)) ** 2
     / (1j * 2 * pi * f_u * DT) ** 2
 )
-fw = fw / dd_decay / zoh_decay
+fw = fw / zoh_decay / zoh_decay  # / zoh_decay
 plt.figure()
 plt.plot(t, u, label="u")
 plt.legend()
@@ -112,22 +112,22 @@
 plt.xscale("log")
 plt.plot(f_u, np.angle(fw, deg=True))
 # dd_decay
-plt.figure(figsize=(14, 4))
-plt.subplot(121)
-plt.xscale("log")
-plt.plot(f_u, 20 * np.log10(np.abs(dd_decay)))
-
-plt.subplot(122)
-plt.xscale("log")
-plt.plot(f_u, np.angle(dd_decay, deg=True))
-
-# dd_decay
-plt.figure(figsize=(14, 4))
-plt.subplot(121)
-plt.xscale("log")
-plt.plot(f_u, 20 * np.log10(np.abs(zoh_decay)))
-
-plt.subplot(122)
-plt.xscale("log")
-plt.plot(f_u, np.angle(zoh_decay, deg=True))
+# plt.figure(figsize=(14, 4))
+# plt.subplot(121)
+# plt.xscale("log")
+# plt.plot(f_u, 20 * np.log10(np.abs(dd_decay)))
+#
+# plt.subplot(122)
+# plt.xscale("log")
+# plt.plot(f_u, np.angle(dd_decay, deg=True))
+#
+# # dd_decay
+# plt.figure(figsize=(14, 4))
+# plt.subplot(121)
+# plt.xscale("log")
+# plt.plot(f_u, 20 * np.log10(np.abs(zoh_decay)))
+#
+# plt.subplot(122)
+# plt.xscale("log")
+# plt.plot(f_u, np.angle(zoh_decay, deg=True))
 plt.show()
diff --git a/test_file.py b/test_file.py
index 72b0ed2..01d380d 100644
--- a/test_file.py
+++ b/test_file.py
@@ -74,8 +74,8 @@
     input_sig = u[i]
     y_output, x_state = sys_ss.response(input_sig, method="zoh2")
     y[i] = y_output
-# y = np.diff(y, 2) / DT / DT
-# y = np.pad(y, (2, 0), "constant", constant_values=(0, 0))
+y = np.diff(y, 2) / DT / DT
+y = np.pad(y, (2, 0), "constant", constant_values=(0, 0))
 
 
 u_detrend = u - np.mean(u)

From fbe1c1038750a1d90140ad89417f20654688b078 Mon Sep 17 00:00:00 2001
From: ChouHor <ichenqiuhao@icloud.com>
Date: Wed, 23 Feb 2022 00:59:46 +0800
Subject: [PATCH 2/2] 1

---
 .DS_Store                                     | Bin 10244 -> 10244 bytes
 pytxt_convertor/.DS_Store                     | Bin 0 -> 6148 bytes
 pytxt_convertor/all_content.txt               |  64 ++++++++++++++++++
 ...lllllllllllllllllllllllllllllllllllllll.py |  26 +++++++
 ...lllllllllllllllllllllllllllllllllllllll.py |  24 +++++++
 pytxt_convertor/test1.py                      |   3 +
 pytxt_convertor/test2.py                      |   3 +
 pytxt_convertor/test3.py                      |   3 +
 sys_id_fft.py                                 |   4 +-
 9 files changed, 125 insertions(+), 2 deletions(-)
 create mode 100644 pytxt_convertor/.DS_Store
 create mode 100644 pytxt_convertor/all_content.txt
 create mode 100644 pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py
 create mode 100644 pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py
 create mode 100644 pytxt_convertor/test1.py
 create mode 100644 pytxt_convertor/test2.py
 create mode 100644 pytxt_convertor/test3.py

diff --git a/.DS_Store b/.DS_Store
index 9091ca9e53fca2e827bad243acb9027cafa7c7b1..3027fd42958c92b93806492b47e1b992e3db71a9 100644
GIT binary patch
delta 70
zcmZn(XbG6$&uF<ZU^hRb<z^m%ADkNe3<V683?&Q|KpfAI%#hEJ$56(Q%1{L4=Q9+g
X6es5-<>%)xZf2Ca&AORg;V(M?#_JR}

delta 32
ocmZn(XbG6$&uFnRU^hRb#bzFXADo-@rEaiJY|!4!uJD%~0KY^FhyVZp

diff --git a/pytxt_convertor/.DS_Store b/pytxt_convertor/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..5008ddfcf53c02e82d7eee2e57c38e5672ef89f6
GIT binary patch
literal 6148
zcmeH~Jr2S!425mzP>H1@V-^m;4Wg<&0T*E43hX&L&p$$qDprKhvt+--jT7}7np#A3
zem<@ulZcFPQ@L2!n>{z**<q8>++&mCkOWA81W14cNZ<zv;LbK1Poaz?KmsK2CSc!(
z0ynLxE!0092;Krf2c+FF_Fe*7ECH>lEfg7;MkzE(HCqgga^y>{tEnwC%0;vJ&^%eQ
zLs35+`xjp>T0<F0fCPF1$Cyrb|F7^5{eNG?83~ZUUlGt@xh*qZDeu<Z%US-OSsOPv
j)R!Z4KLME7ReXlK;d!wEw5GODWMKRea10D2@KpjYNUI8I

literal 0
HcmV?d00001

diff --git a/pytxt_convertor/all_content.txt b/pytxt_convertor/all_content.txt
new file mode 100644
index 0000000..3ce8472
--- /dev/null
+++ b/pytxt_convertor/all_content.txt
@@ -0,0 +1,64 @@
+;start_character;test1.py;end_character;
+import numpy as np
+
+print(1)
+;start_character;test2.py;end_character;
+import numpy as np
+
+print(1)
+;start_character;test3.py;end_character;
+import numpy as np
+
+print(1)
+;start_character;llllllllllllllllllllllllllllllllllllllllll.py;end_character;
+def get_text_file(filename):
+    f = open(filename, "r")
+    _content = f.read()
+    f.close()
+    return _content
+
+
+all_content = get_text_file("all_content.txt")
+contents = all_content.split(";start_character;")[1:]
+num = len(contents)
+for i in range(num - 1):
+    if i < num - 2:
+        content = contents[i]
+        file_contents = content.split((";end_character;\n"))
+
+    else:
+        content = contents[i]
+        file_contents = content.split((";end_character;\n"))
+        file_contents[1] = file_contents[1] + contents[i + 1]
+    file_name = "1" + file_contents[0]
+
+    file_content = file_contents[1]
+
+    fh = open(file_name, "w", encoding="utf-8")
+    fh.write(file_content)
+    fh.close()
+;start_character;lllllllllllllllllllllllllllllllllllllllllll.py;end_character;
+import os
+
+
+def get_text_file(filename):
+    f = open(filename, "r")
+    _content = f.read()
+    f.close()
+    return _content
+
+
+path = os.getcwd()
+files = os.listdir(path)
+files.sort(key=len)
+all_content = ""
+for file_name in files:
+    if file_name[-3:] == ".py":
+        start_character = ";start_character;" + file_name + ";end_character;\n"
+        content = get_text_file(file_name)
+        new_content = start_character + content
+        all_content = all_content + new_content
+
+fh = open("all_content.txt", "w", encoding="utf-8")
+fh.write(all_content)
+fh.close()
diff --git a/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py b/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py
new file mode 100644
index 0000000..9f9e229
--- /dev/null
+++ b/pytxt_convertor/llllllllllllllllllllllllllllllllllllllllll.py
@@ -0,0 +1,26 @@
+def get_text_file(filename):
+    f = open(filename, "r")
+    _content = f.read()
+    f.close()
+    return _content
+
+
+all_content = get_text_file("all_content.txt")
+contents = all_content.split(";start_character;")[1:]
+num = len(contents)
+for i in range(num - 1):
+    if i < num - 2:
+        content = contents[i]
+        file_contents = content.split((";end_character;\n"))
+
+    else:
+        content = contents[i]
+        file_contents = content.split((";end_character;\n"))
+        file_contents[1] = file_contents[1] + contents[i + 1]
+    file_name = "1" + file_contents[0]
+
+    file_content = file_contents[1]
+
+    fh = open(file_name, "w", encoding="utf-8")
+    fh.write(file_content)
+    fh.close()
diff --git a/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py b/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py
new file mode 100644
index 0000000..8e91fc2
--- /dev/null
+++ b/pytxt_convertor/lllllllllllllllllllllllllllllllllllllllllll.py
@@ -0,0 +1,24 @@
+import os
+
+
+def get_text_file(filename):
+    f = open(filename, "r")
+    _content = f.read()
+    f.close()
+    return _content
+
+
+path = os.getcwd()
+files = os.listdir(path)
+files.sort(key=len)
+all_content = ""
+for file_name in files:
+    if file_name[-3:] == ".py":
+        start_character = ";start_character;" + file_name + ";end_character;\n"
+        content = get_text_file(file_name)
+        new_content = start_character + content
+        all_content = all_content + new_content
+
+fh = open("all_content.txt", "w", encoding="utf-8")
+fh.write(all_content)
+fh.close()
diff --git a/pytxt_convertor/test1.py b/pytxt_convertor/test1.py
new file mode 100644
index 0000000..756cbb9
--- /dev/null
+++ b/pytxt_convertor/test1.py
@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)
diff --git a/pytxt_convertor/test2.py b/pytxt_convertor/test2.py
new file mode 100644
index 0000000..756cbb9
--- /dev/null
+++ b/pytxt_convertor/test2.py
@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)
diff --git a/pytxt_convertor/test3.py b/pytxt_convertor/test3.py
new file mode 100644
index 0000000..756cbb9
--- /dev/null
+++ b/pytxt_convertor/test3.py
@@ -0,0 +1,3 @@
+import numpy as np
+
+print(1)
diff --git a/sys_id_fft.py b/sys_id_fft.py
index 8a9beac..75629bf 100644
--- a/sys_id_fft.py
+++ b/sys_id_fft.py
@@ -38,7 +38,7 @@
 
 """Chirp参数"""
 start_freq = 50
-end_freq = 5000
+end_freq = 5100
 start_freq_ = 0.8 * start_freq
 end_freq_ = 1.1 * end_freq
 # 扫频时间
@@ -57,7 +57,7 @@
 y = np.zeros_like(u)
 for i in range(len(u)):
     input_sig = u[i]
-    y_output, x_state = ss.response(input_sig, method="zoh")
+    y_output, x_state = ss.response(input_sig, method="linear")
     y[i] = y_output
 p = y
 y = np.diff(y, 2) / DT / DT