package Ġjp . seo . station . ek is ag as u . position čĊč Ċ import Ġorg . apache . commons . math 3 . linear . Matrix Utils č Ċ import Ġorg . apache . commons . math 3 . linear . Real Matrix čĊčĊč Ċ /** čĊ Ġ* Ġ@ author ĠSe o - 4 d 6 9 6 b 7 5 čĊ Ġ* Ġ@ version Ġ 2 0 2 0 / 0 6 / 2 8 . čĊ Ġ*/ č Ċ class ĠKal man Filter Ġ{ čĊčĊĠĠĠ Ġcompanion Ġobject Ġ{ čĊčĊĠĠĠĠĠĠĠ Ġ/** čĊĠĠĠĠĠĠĠĠ Ġ* Ġhttps :// ja . wikipedia . org / wiki /% E 8 % B 5 % B 7 % E 5 % 8 B % 9 5 % E 5 % 8 A % A 0 % E 9 % 8 0 % 9 F % E 5 % BA % A 6 čĊĠĠĠĠĠĠĠĠ Ġ* Ġ éĽ» è» Ĭ ãģ® èµ· åĭķ åĬł éĢŁåº¦ ãģ¯ ãģĽ ãģĦ ãģ ľ ãģĦ 5 . 0 km / h / s čĊĠĠĠĠĠĠĠĠ Ġ* Ġæ¨ Ļ æºĸ æŃ£ è¦ı åĪĨå¸ĥ ãģ® ç´ ¯ ç© į ç¢º çİĩ åĪĨå¸ĥ éĸ¢æķ° ãģ§ 9 7 . 5 % çĤ¹ ãģ¯ 1 . 9 7 čĊĠĠĠĠĠĠĠĠ Ġ* Ġæ¨ Ļ æºĸ åģı å·® 2 . 5 km / h / s ãģ Ĳ ãĤī ãģĦ Ġ~ Ġ 1 . 0 m / s ^ 2 Ġãģ® ãĤª ãĥ¼ãĥ Ģ ãĥ¼ čĊĠĠĠĠĠĠĠĠ Ġ*/ čĊĠĠĠĠĠĠĠ Ġconst Ġval ĠSIG MA Ġ= Ġ 1 . 0 čĊĠĠĠ Ġ} čĊčĊĠĠĠ Ġclass ĠSample ( čĊĠĠĠĠĠĠĠ Ġval Ġtime : ĠLong , čĊĠĠĠĠĠĠĠ Ġval Ġpos : ĠDouble , čĊĠĠĠĠĠĠĠ Ġval Ġspeed : ĠDouble , čĊĠĠĠĠĠĠĠ Ġval Ġacceleration : ĠDouble , čĊĠĠĠĠĠĠĠ Ġvar Ġstate : ĠReal Matrix , čĊĠĠĠĠĠĠĠ Ġvar Ġp : ĠReal Matrix čĊĠĠĠ Ġ) Ġ: ĠClone able Ġ{ čĊčĊĠĠĠĠĠĠĠ Ġ@ Throws ( Clone NotSupportedException :: class ) čĊĠĠĠĠĠĠĠ Ġpublic Ġoverride Ġfun Ġclone (): ĠSample Ġ{ čĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġother Ġ= Ġsuper . clone () Ġas ĠSample čĊĠĠĠĠĠĠĠĠĠĠĠ Ġother . state Ġ= Ġstate . copy () čĊĠĠĠĠĠĠĠĠĠĠĠ Ġother . p Ġ= Ġp . copy () čĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġother čĊĠĠĠĠĠĠĠ Ġ} čĊčĊĠĠĠĠĠĠĠ Ġcompanion Ġobject Ġ{ čĊĠĠĠĠĠĠĠĠĠĠĠ Ġ/** čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ åĪĿ æľŁ åĮĸ ãģĹãģŁ çĬ¶æħĭ ãĤª ãĥĸãĤ¸ãĤ§ãĤ¯ãĥĪ ãĤĴ è¿Ķ ãģĻ čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġtime Ġæ Ļ Ĥ åĪ» čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġpos Ġ è¦ ³ æ¸ ¬ çı¾åľ¨ ä½įç½® čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġspeed Ġ è¦ ³ æ¸ ¬ çı¾åľ¨ éĢŁåº¦ , Ġ è¦ ³ æ¸ ¬ åĢ¤ æ¬ ł æĲ į ãģ® åł´åĲĪãģ¯ 0 ï¼Ī éģ© å½ĵ ãģª åĢ¤ ï¼ī čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġvariance Ġ ä½įç½® ãĥ» éĢŁåº¦ ãģ® è¦ ³ æ¸ ¬ åĢ¤ ãģ® å¹³åĿĩ åĪĨ æķ£ >= 0 ãĢģ åĢ¤ ãģĮ å¤§ ãģį ãģĦ ãģ » ãģ© è¦ ³ æ¸ ¬ ç²¾ åº¦ ãģĮ ä½İ ãģĦ ãģ¨ è§£ éĩ Ī ãģķãĤĮãĤĭ čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ* Ġ@ return čĊĠĠĠĠĠĠĠĠĠĠĠĠ Ġ*/ čĊĠĠĠĠĠĠĠĠĠĠĠ Ġfun Ġinitialize ( time : ĠLong , Ġpos : ĠDouble , Ġspeed : ĠDouble , Ġvariance : ĠDouble ): ĠSample Ġ{ čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġv Ġ= Ġvariance . coerce At Least ( 0 . 0 ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġstate Ġ= ĠMatrix Utils . create Real Matrix ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠarrayOf ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( pos ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( speed ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 0 . 0 ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġp Ġ= ĠMatrix Utils . create Real Matrix ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ ĠarrayOf ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( v , Ġ 0 . 0 , Ġ 0 . 0 ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 0 . 0 , Ġv , Ġ 0 . 0 ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 0 . 0 , Ġ 0 . 0 , Ġv ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĠSample ( time , Ġpos , Ġspeed , Ġ 0 . 0 , Ġstate , Ġp ) čĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} čĊĠĠĠĠĠĠĠ Ġ} čĊĠĠĠ Ġ} čĊčĊĠĠĠ Ġprivate Ġval Ġm H : ĠReal Matrix Ġ= ĠMatrix Utils . create Real Matrix ( array Of ( double ArrayOf ( 1 . 0 , Ġ 0 . 0 , Ġ 0 . 0 ))) čĊčĊĠĠĠ Ġfun Ġupdate ( pos : ĠDouble , Ġerr : ĠDouble , Ġtime : ĠLong , Ġlatest : ĠSample ): ĠSample Ġ{ čĊĠĠĠĠĠĠĠ Ġval Ġdelta Ġ= Ġ( time Ġ- Ġlatest . time ) Ġ/ Ġ 1 0 0 0 . 0 čĊĠĠĠĠĠĠĠ Ġval Ġf Ġ= ĠMatrix Utils . create Real Matrix ( čĊĠĠĠĠĠĠĠĠĠĠĠ ĠarrayOf ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 1 . 0 , Ġdelta , Ġdelta Ġ* Ġdelta Ġ/ Ġ 2 ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 0 . 0 , Ġ 1 . 0 , Ġdelta ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( 0 . 0 , Ġ 0 . 0 , Ġ 1 . 0 ) čĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠ Ġval Ġg Ġ= ĠMatrix Utils . create Real Matrix ( čĊĠĠĠĠĠĠĠĠĠĠĠ ĠarrayOf ( čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( delta Ġ* Ġdelta Ġ* Ġdelta Ġ/ Ġ 6 ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( delta Ġ* Ġdelta Ġ/ Ġ 2 ), čĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġdouble ArrayOf ( delta ) čĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠ Ġ) čĊĠĠĠĠĠĠĠ Ġ// estimate čĊĠĠĠĠĠĠĠ Ġval Ġestimated State Ġ= Ġf . multiply ( latest . state ) čĊĠĠĠĠĠĠĠ Ġval Ġp Ġ= Ġf . multiply ( latest . p ). multiply ( f . transpose ()) čĊĠĠĠĠĠĠĠĠĠĠĠ Ġ. add ( g . multiply ( g . transpose ()). scalar Multiply ( SIG MA Ġ* ĠSIG MA )) čĊĠĠĠĠĠĠĠ Ġ// update čĊĠĠĠĠĠĠĠ Ġval Ġe Ġ= Ġpos Ġ- Ġm H . multiply ( estimated State ). getEntry ( 0 , Ġ 0 ) čĊĠĠĠĠĠĠĠ Ġval Ġs Ġ= Ġerr Ġ* Ġerr Ġ+ Ġm H . multiply ( p ). multiply ( m H . transpose ()). getEntry ( 0 , Ġ 0 ) čĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġp . multiply ( m H . transpose ()). scalar Multiply ( 1 . 0 Ġ/ Ġs ) čĊĠĠĠĠĠĠĠ Ġval Ġnext State Ġ= Ġestimated State . add ( k . scalar Multiply ( e )) čĊĠĠĠĠĠĠĠ Ġval Ġnext P Ġ= ĠMatrix Utils . create Real Identity Matrix ( 3 ). subtract ( k . multiply ( m H )). multiply ( p ) čĊĠĠĠĠĠĠĠ Ġreturn ĠSample ( čĊĠĠĠĠĠĠĠĠĠĠĠ Ġtime , čĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext State . getEntry ( 0 , Ġ 0 ), čĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext State . getEntry ( 1 , Ġ 0 ), čĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext State . getEntry ( 2 , Ġ 0 ), čĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext State , čĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext P čĊĠĠĠĠĠĠĠ Ġ) čĊĠĠĠ Ġ} čĊč Ċ } čĊ