├── LICENSE ├── README.md ├── example ├── car │ ├── car.R │ ├── car.go │ └── car.png └── rose │ ├── p133.csv │ ├── rose.R │ ├── rose.go │ └── rose.png ├── go.mod ├── go.sum ├── kalman.go ├── kalman_test.go ├── matlab ├── Kalman_Filter_Test_Seite_145_Kapitel_10_2.m ├── Rose_Filter_Test_Seite_133_Kapitel_9_4.m └── Seite_145_Kapitel_10_2_data_t_y.csv ├── rose.go └── rose_test.go /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright © 2019 Kalkfabrik Netstal AG, 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the “Software”), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is furnished to do 10 | so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Adaptive Kalman filtering in Golang 2 | 3 | [![License](http://img.shields.io/badge/license-MIT-red.svg?style=flat)](https://github.com/konimarti/kalman/blob/master/LICENSE) 4 | [![GoDoc](https://godoc.org/github.com/konimarti/observer?status.svg)](https://godoc.org/github.com/konimarti/kalman) 5 | [![goreportcard](https://goreportcard.com/badge/github.com/konimarti/observer)](https://goreportcard.com/report/github.com/konimarti/kalman) 6 | 7 | ```go get github.com/konimarti/kalman``` 8 | 9 | * Adaptive Kalman filtering with Rapid Ongoing Stochastic covariance Estimation (ROSE) 10 | 11 | * A helpful introduction to how Kalman filters work, can be found [here](https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/). 12 | 13 | * Kalman filters are based on a state-space representation of linear, time-invariant systems: 14 | 15 | The next state is defined as 16 | ```math 17 | x(t+1) = A_d * x(t) + B_d * u(t) 18 | ``` 19 | where A_d is the discretized prediction matrix and B_d the control matrix. 20 | x(t) is the current state and u(t) the external input. The response (measurement) of the system is y(t): 21 | ```math 22 | y(t) = C * x(t) + D * u(t) 23 | ``` 24 | 25 | ## Using the standard Kalman filter 26 | ```go 27 | // create filter 28 | filter := kalman.NewFilter( 29 | lti.Discrete{ 30 | Ad, // prediction matrix (n x n) 31 | Bd, // control matrix (n x k) 32 | C, // measurement matrix (l x n) 33 | D, // measurement matrix (l x k) 34 | }, 35 | kalman.Noise{ 36 | Q, // process model covariance matrix (n x n) 37 | R, // measurement errors (l x l) 38 | }, 39 | ) 40 | 41 | // create context 42 | ctx := kalman.Context{ 43 | X, // initial state (n x 1) 44 | P, // initial process covariance (n x n) 45 | } 46 | 47 | // get measurement (l x 1) and control (k x 1) vectors 48 | .. 49 | 50 | // apply filter 51 | filteredMeasurement := filter.Apply(&ctx, measurement, control) 52 | } 53 | ``` 54 | 55 | ### Results with standard Kalman filter 56 | 57 | ![Results of Kalman filtering on car example.](example/car/car.png) 58 | 59 | See example [here](example/car/car.go). 60 | 61 | ### Results with Rapid Ongoing Stochasic covariance Estimation (ROSE) filter 62 | 63 | ![Results of ROSE filtering.](example/rose/rose.png) 64 | 65 | See example [here](example/rose/rose.go). 66 | 67 | ### Math behind the Kalman filter 68 | 69 | * Calculation of the Kalman gain and the correction of the state vector ~x(k) and covariance matrix ~P(k): 70 | ```math 71 | ^y(k) = C * ^x(k) + D * u(k) 72 | dy(k) = y(k) - ^y(k) 73 | K(k) = ^P(k) * C^T * ( C * ^P(k) * C^T + R(k) )^(-1) 74 | ~x(k) = ^x(k) + K(k) * dy(k) 75 | ~P(k) = ( I - K(k) * C) * ^P(k) 76 | ``` 77 | * Then the next step is predicted for the state ^x(k+1) and the covariance ^P(k+1): 78 | ```math 79 | ^x(k+1) = Ad * ~x(k) + Bd * u(k) 80 | ^P(k+1) = Ad * ~P(k) * Ad^T + Gd * Q(k) * Gd^T 81 | ``` 82 | 83 | 84 | 85 | ## Credits 86 | 87 | This software package has been developed for and is in production at [Kalkfabrik Netstal](http://www.kfn.ch/en). 88 | -------------------------------------------------------------------------------- /example/car/car.R: -------------------------------------------------------------------------------- 1 | df<-read.csv(file="car.csv", header=T) 2 | 3 | png("car.png") 4 | 5 | op<-par(mfrow=c(2,1), 6 | mar = c(3,4,1,1)+0.1) 7 | 8 | plot(df$Measured_v_x, ylab="V_x", xlab="Time",type="l") 9 | lines(df$Filtered_v_x,col="red",lwd=2) 10 | legend("topright", legend=c("Measured","Filtered"), lty=1, col=c("black","red")) 11 | 12 | plot(df$Measured_v_y, ylab="V_y", xlab="Time",type="l") 13 | lines(df$Filtered_v_y,col="green",lwd=2) 14 | legend("topright", legend=c("Measured","Filtered"), lty=1, col=c("black","green")) 15 | 16 | par(op) 17 | 18 | dev.off() 19 | -------------------------------------------------------------------------------- /example/car/car.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "fmt" 5 | "math" 6 | "math/rand" 7 | "os" 8 | 9 | "github.com/konimarti/kalman" 10 | "github.com/konimarti/lti" 11 | "gonum.org/v1/gonum/mat" 12 | ) 13 | 14 | func main() { 15 | // prepare output file 16 | file, err := os.Create("car.csv") 17 | if err != nil { 18 | panic(err) 19 | } 20 | defer file.Close() 21 | 22 | fmt.Fprintln(file, "Measured_v_x,Measured_v_y,Filtered_v_x,Filtered_v_y") 23 | 24 | ctx := kalman.Context{ 25 | // init state: pos_x = 0, pox_y = 0, v_x = 30 km/h, v_y = 10 km/h 26 | X: mat.NewVecDense(4, []float64{0, 0, 30, 10}), 27 | // initial covariance matrix 28 | P: mat.NewDense(4, 4, []float64{ 29 | 1, 0, 0, 0, 30 | 0, 1, 0, 0, 31 | 0, 0, 1, 0, 32 | 0, 0, 0, 1}), 33 | } 34 | 35 | // time step 36 | dt := 0.1 37 | 38 | lti := lti.Discrete{ 39 | // prediction matrix 40 | Ad: mat.NewDense(4, 4, []float64{ 41 | 1, 0, dt, 0, 42 | 0, 1, 0, dt, 43 | 0, 0, 1, 0, 44 | 0, 0, 0, 1, 45 | }), 46 | // no external influence 47 | Bd: mat.NewDense(4, 4, nil), 48 | // scaling matrix for measurement 49 | C: mat.NewDense(2, 4, []float64{ 50 | 0, 0, 1, 0, 51 | 0, 0, 0, 1, 52 | }), 53 | // scaling matrix for control 54 | D: mat.NewDense(2, 4, nil), 55 | } 56 | 57 | // G 58 | G := mat.NewDense(4, 2, []float64{ 59 | 0, 0, 60 | 0, 0, 61 | 1, 0, 62 | 0, 1, 63 | }) 64 | var Gd mat.Dense 65 | Gd.Mul(lti.Ad, G) 66 | 67 | // process model covariance matrix 68 | qk := mat.NewDense(2, 2, []float64{ 69 | 0.01, 0, 70 | 0, 0.01, 71 | }) 72 | var Q mat.Dense 73 | Q.Product(&Gd, qk, Gd.T()) 74 | 75 | // measurement errors 76 | corr := 0.5 77 | R := mat.NewDense(2, 2, []float64{1, corr, corr, 1}) 78 | 79 | // create noise struct 80 | nse := kalman.Noise{&Q, R} 81 | 82 | // create Kalman filter 83 | filter := kalman.NewFilter(lti, nse) 84 | 85 | // no control 86 | control := mat.NewVecDense(4, nil) 87 | 88 | for i := 0; i < 200; i++ { 89 | x1 := rand.NormFloat64() 90 | x2 := rand.NormFloat64() 91 | x3 := corr*x1 + math.Sqrt(1-corr)*x2 92 | y1 := 30.0 + 1.0*x1 93 | y2 := 10.0 + 1.0*x3 94 | // measure v_x and v_y with an error which is distributed according to stanard normal 95 | measurement := mat.NewVecDense(2, []float64{y1, y2}) 96 | 97 | // apply filter 98 | filtered := filter.Apply(&ctx, measurement, control) 99 | 100 | // print out 101 | fmt.Fprintf(file, "%3.8f,%3.8f,%3.8f,%3.8f\n", measurement.AtVec(0), measurement.AtVec(1), filtered.AtVec(0), filtered.AtVec(1)) 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /example/car/car.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/konimarti/kalman/25097b2d79d4a6ded52722bbfef9350957bcfaaf/example/car/car.png -------------------------------------------------------------------------------- /example/rose/p133.csv: -------------------------------------------------------------------------------- 1 | 0.02003230190216821,0.01739130329595394,0.1725892988718226,-0.1155996650060248,0.02155939824584764,-0.02952495747858654,0.04965269839809141,-0.1283296205706978,-0.02880698297470682,-0.02066319445346404,0.07801152185913179,0.03074777511670306,0.01179139120845439,0.09858947462741247,0.1394678297075801,-0.02780735858039539,0.02953309394685306,-0.08993972721012189,-0.05276630794659748,-0.04734559154470904,-0.005842903857438776,0.0656968445237773,-0.03400155950131017,0.1434641012840426,0.0386699676524314,0.04232616662277359,-0.03630642270804207,-0.1636553473373458,0.1073071117491785,-0.01863860158250331,0.02505203662826134,0.003756034934860486,-0.06843328546099614,0.2549202469727693,0.1574174325738794,-0.06218227097015454,0.08715638057477415,0.01837631068936963,0.08668612453824413,0.02421402394570574,0.03915445922545713,-0.04544259745381977,-0.08099476915976293,-0.05818041085368401,0.04292746728028141,0.06479807859595167,-0.03480294349332352,-0.05446752830045735,0.04566548056990517,0.01594667743545962,0.06384010112343724,0.06857809938707579,-0.0124642727145119,0.1180221832771133,0.04474410481338183,0.06793833495084065,-0.02045190577102417,-0.01358683282312813,-0.03400351475505346,-0.01908697505471068,0.06230280434189303,0.01232686631752299,0.1065307054157112,-0.03083558443684034,0.008658676491512571,0.09606522861207585,0.05120823393697546,0.01605644692453996,-0.04473066328279007,0.04769258155340036,0.1349547012827162,-0.03004314798623732,-0.06264965250812002,0.1059954621176351,0.08905434949740666,0.01437340452261963,0.02580349564841536,0.05805720820805584,-0.001373826337284105,0.03445665272073524,0.03290195268258108,-0.1001182281781483,0.07738360608058463,0.06983560417447095,0.04489779658011669,-0.02854180568974805,0.09689624618339841,0.04121173767723032,0.00923556566112951,0.04228696608396144,0.02426389171626752,-0.05681821679785268,-0.08169902687420774,0.01878643080860061,0.09984481576006653,-0.02608278307797441,-0.02652542205860614,0.072624433926349,0.0437873389724787,-0.03966406799996582,0.553768565113791,0.3990600744021808,0.3772196012965454,0.518308916668486,0.4993109890945882,0.6474196721985608,0.4110101983325133,0.4699894352646795,0.5683243013344772,0.5351139571773892,0.4540967489537657,0.6204159429894505,0.4854590405699543,0.3967816846173535,0.5453489757660169,0.4218962606306987,0.5920397988123225,0.4579239294169303,0.4511331013729029,0.6238290288958444,0.6178199242269744,0.5094539856411802,0.4488913235789438,0.6872334043399722,0.5615048829032933,0.5069268153878655,0.5842515400683121,0.3941388914894062,0.3906105578499541,0.5330343249489051,0.4405106614969271,0.4225997376159671,0.6412409506628801,0.4862259717160046,0.5237106981884391,0.5592208780927028,0.5221812782638947,0.5643970782228269,0.5158450001246384,0.6100695826140342,0.5153861316883428,0.4850506292477501,0.5325525522811536,0.4942708839291097,0.6197148295605645,0.4500523957131746,0.6550480413528655,0.504544947254617,0.6086245972464994,0.6061365069168163,0.5224064349099127,0.5667513806168152,0.5109401925670974,0.5919694212817034,0.516607625719476,0.5294892172284658,0.4863474985839407,0.5281484976753638,0.436626503049729,0.4275290561957943,0.4608021511773135,0.4846268233715068,0.4584896975192447,0.5196907933077133,0.5469592426350922,0.5467762620715022,0.5406828572399563,0.5698492174084142,0.5180162932364054,0.4320946587955575,0.4740052653431856,0.5099888575563603,0.5291969585863057,0.5392104455216872,0.4102447734013278,0.4880601407821267,0.5389252948247581,0.4829362401812825,0.562185535113863,0.4945810916716427,0.5218015706063232,0.5112538841339899,0.5958997150077097,0.4171867158492745,0.6394426233760899,0.4356081343396618,0.4225150629228881,0.5378015442209764,0.5301718310279568,0.4946050209449491,0.4165040781606106,0.4825049200923677,0.4451932560615692,0.3758617980041598,0.4880613365428967,0.5055925441552187,0.5061996243485466,0.3796337749348179,0.5521669723922527,0.4045076023131199,0.6209394447394129,0.4989738911097661,0.4437389681550887,0.425457119510833,0.5980779690372438,0.4459453220281854,0.4558356390457399,0.5012062645767524,0.3940396647589109,0.5400681970672784,0.5565045793644701,0.4693011472645334,0.6259389241262963,0.5055076562005377,0.4284638704509505,0.5258468529802592,0.4894568363655377,0.5209466975815715,0.4988433914022145,0.5224789193332291,0.5074041520434051,0.4552658424867573,0.512738517039655,0.4671297182877938,0.4249027608919895,0.4812035049046458,0.4389454991279092,0.4389564257024965,0.4252652209915122,0.4779189967865241,0.4634380972426713,0.4815569636686635,0.5241579276097691,0.4400598657390306,0.5691736421838329,0.4324839010307167,0.4746208257119462,0.571397528131856,0.4952126236400229,0.5980684090891004,0.4943079649971969,0.3022799716181227,0.4840102537508233,0.5001488175677682,0.5212042738435869,0.4036243268240414,0.4984072553719012,0.5139792590645017,0.4641208205370858,0.2955771824072474,0.4613944070706668,0.7280098353350386,0.5224314827976185,0.7767940663102035,0.2587178351815936,0.7449634460189978,0.8826997384644464,0.4847315123271491,0.1671700222119821,-0.07270257581230422,0.8879309263560782,0.9899026819412605,0.4527274472061974,0.1980914117652298,0.265389885095061,0.5366919350727841,0.7253066347428656,0.5647779760541636,0.7200943886003073,0.8301944717803498,0.3487305046997304,-0.06577902720836215,0.137499271920571,0.4610882553884161,0.465843517087714,0.3576662394351136,0.04135203606005861,0.440691782254767,0.516734451955764,0.1150810376504182,0.6020084109150547,0.8288721849259315,0.6857712403903232,0.3956524000754058,0.8444730017151145,0.7695923527049,0.6711908052895856,0.7720257447872322,0.8870160701067813,0.3371816261391023,0.8844182440942934,0.2231845085945623,0.8268131666971079,0.6783679752712164,0.5018335844900236,0.3357126451330602,0.7260927164455004,0.9653774801461552,0.3317977551960232,0.7974366246502119,0.03717211679553016,-0.341395077972059,0.4758517261162086,-0.08978407800737584,0.2740775110965882,0.3452786252220845,0.387799947666401,0.2013517612795772,-0.07515341877706401,-0.5495787057745044,-0.3525349301146261,-0.3545976075061846,-0.1944700668908466,0.3947673200972115,-0.2499248711732726,-0.1132165708568874,0.08858055147069831,-0.1521280764449102,-0.1958680405862454,0.4623266822762753,0.06170138465760257,-0.1696668458462836,-0.2135281170613263,0.257465781415179,0.07252930282682908,0.01300276252608172,0.1213242879083018,0.1987705153287343,0.07707750582304562,0.1782745062946286,0.1118228182065304,-0.09794393147460807,0.03382406686487909,0.04726473128684107,-0.1863941123162841,-0.07492416396456453,-0.3055881310985221,-0.1929481401844322,-0.05787104893986037,-0.0486783784263042,0.1090778742187284,-0.1208451302369099,0.2885633022384148,-0.03000389894001022,-0.4726475523081461,0.01675559396348732,-0.0434305032682358,0.1744267163524728,0.3474013021914674,0.1287233228664365,0.03025355261783171,-0.1751739622219962,-0.3678289935178358,-0.1263516063378946,0.345905776770402,-0.1858541900030083,-0.02137821516943627,-0.295422687109442,-0.1035004153314667,0.08091265048531981,0.04530600751704209,-0.03588526816298712,0.4290446791868954,-0.2217167884803802,-0.2170329002767518,0.3385565554245376,0.08144109760491168,0.009357432978775565,-0.05467729634318896,-0.1700994997020072,0.5192000709451936,-0.04605130420373155,0.1748043991003049,-0.1307236620608979,0.3634990497760746,0.1918157132867654,-0.1409848247955625,-0.1106691196375622,-0.08702467611841538,0.1737309726568796,0.2191145612048682,-0.04082158563004851,0.1305458322906921,0.3563521510733506,-0.07650034878169748,0.2304917368389369,0.2199708905213456,-0.1486736521570803,0.2800314204159132,0.2836143444698655,0.109611608151449,-0.02443360938233588,0.2841863979400315,-0.2045739755617539,-0.5211908997915613,0.2972274691211015,0.05964405003937946,-0.158460562351472,-0.08386502846593918,0.143747355661875,-0.04056860145965008,-0.2617149156062308,-0.1536247837368534,-0.02135371287815214,-0.2661692546668015,0.09776775885806559,0.2107461223756331,-0.07886853005269104,-0.195825885721966,-0.02984640531218476,-0.1441753648525048,0.08337152393235155,-0.01598310000736373,-0.1402564719146101,-0.151833223010661,-0.221350736857966,-0.1359809255773292,0.02062708695339617,-0.2611817043671181,-0.5109549473055057,0.09322016377554933,0.06144195433520343,0.3577758183976788,-0.312950211594321,0.4287144703960494,0.4031288860165116,-0.3069422261803602,0.01137651233385831,-0.01651712502366646,-0.1043086593668472,-0.07512458286160184,0.07664249723069474,0.04002739048195492,-0.2761966138399074,-0.06050664675846831,-0.1956169819145016,-0.3288321773358643,-0.4118541082553926,-0.1061987597840114,0.1711052823112673,-0.2108880957505125,-0.0275266476767012,-0.04497053395116503,0.2764663446852749,-0.2897986273637401,0.01655069295587617,-0.1533401457401105,-0.5374079066124121,-0.03250080959005219,0.3668862971384345,1.438444222242081,1.241584556223466,1.218392937030841,0.7214007283475998,0.8226123970952498,0.7777414338056811,0.400852568686149,0.9872555700717536,1.29255186784289,1.210326808648473,1.283963892277178,1.369288415911764,0.9713627251642926,0.8344489743805664,0.8831749640384651,1.115949251899287,0.4575848189916349,1.210750755567111,1.535529339038695,1.076415495952954,1.231559741295042,1.000491283797757,1.007803623621633,1.220021784937648,1.137419932855187,1.065161258316695,0.9482301592254285,1.072365013242853,0.4951833069822694,1.005835764442224,1.443878755599503,0.8468943373623192,0.9488056562508009,0.7795910361956093,0.9832961182439313,1.243628750203585,0.7654114834094259,1.423226763067834,0.8259286171467641,1.015242477169582,0.9998528613593388,1.354116358525881,0.7561213419293938,1.041854475089258,1.407055979297296,0.8051911693344198,0.680509265459905,1.108802147824063,0.9526720907121915,1.337606465105773,1.345475317763156,1.129002635042662,1.303079582249009,1.360286004046698,1.123692533899492,0.9545059937874243,0.998934855610405,1.046364227529616,1.362545390112651,1.207833927534926,0.8908603305993974,0.954290810399073,0.8605220643811827,1.194083519629328,0.8552792937752043,1.495888305153565,1.264314302948427,1.282865805286508,0.9107199997723623,1.116340591159241,1.202112929792491,1.185207752738154,0.9573658723671484,0.878713068762608,1.136607012551145,0.8747412764507499,1.155621976361255,1.205713557554106,0.6875413145796475,1.019911190227115,1.323739132083506,0.9464100633525508,1.168732134393127,0.9303299272512482,1.12700164852659,1.068399360580126,0.7990681026451574,1.120851787992388,1.06074081395587,0.8693172260444008,0.8904655124388319,0.9612549922528244,0.7392865313080097,0.7549824901243537,0.632344324563231,1.114547689683374,0.8982671865815169,1.405718896656458,0.9179899703023801,1.403184672890954,1.30164985399368,1.128764858567225,1.139719441397518,0.9849114049092983,0.8266085876338883,1.00065681751288,1.140700767959348,0.8463690498389584,0.7636225463628263,0.8948289280117514,1.368522023934361,1.414034986908728,0.8178952818154207,0.7492751161765383,0.3009357971182883,0.901300681244925,1.391512302432491,0.6046375614291131,1.107309348470302,1.109584399702753,0.9855139238890732,1.450021295168829,0.8546379892712097,0.7937774758268448,1.264267102703908,0.8379120348231172,0.878343538464929,0.6224972499824972,0.9708444260228956,0.9395027915348331,1.217423673729148,1.074313881335668,1.13176139031149,1.121947534871408,1.052202619466128,1.213351650272549,1.138114092169936,0.9356952096873175,0.8743818185878838,0.7041684748138564,0.7879017648582013,1.188345775789938,1.242303751941665,1.160056087067796,0.9859198146632387,0.7701600337185306,0.8071624134840041,1.032769625398494,0.7891864181455215,0.9029821958483574,0.05046555974382204,0.02979408023117086,-0.05058275796885296,-0.1311623531530911,0.01783181196504348,0.1582031912624397,0.09286122909485671,-0.05399452681426028,-0.04364337578079336,-0.04320034442068355,0.06196441976038239,0.08983280960773497,-0.04679203457944027,-0.03460323074533466,0.08990856380685587,-0.07218244186151178,-0.0009158986618763828,0.03076987999804513,-0.04925850820363454,0.1103622693876582,0.0300124993367588,-0.01287091411153115,-0.03404893096928827,0.02578871837995129,-0.06905000010029845,-0.05681333833874542,0.08736039990607425,0.1045758768028148,-0.02509597039359168,0.05988554852057037,0.009861430328018074,0.02509033426314697,-0.04728257742050582,-0.03150142498050296,-0.1039583108690999,-0.0668835240575637,-0.0350641296702826,0.003089828102030415,-0.1086146013381941,-0.1161499693879579,0.06850886740079781,-0.1708855894930767,-0.1205787895997888,0.06553116216579274,-0.0784508713971445,0.06670816199088056,-0.05049878413362796,-0.01564795230805975,0.06023930026809479,-0.03144572440262864,-0.05462604154206833,-0.007218348900535242,-0.02310969579940762,0.2329374584418838,-0.1342255150700596,-0.02518728711783488,0.04718074827514916,-0.007239198327829034,-0.127573311030293,-0.05748541849294218,-0.04339999288875946,-0.1437748906221842,0.04499521550531938,-0.09629298709467783,0.08476037627510501,0.04119961303065569,0.07457043543286683,-0.030178965602445,0.03463881831317066,0.001376929284419088,0.02144983065700885,-0.06229641554818154,0.06320230294119757,-0.06237106068462487,0.08913350332339508,0.04300710468025758,-0.05632873186634157,-0.01051021945714417,0.0499065778131459,-0.03514125562814744,0.09053702118078297,0.0519797191033282,-0.01085769436444136,0.1371969066216887,0.02087082382898648,-0.02675014025492919,-0.06928447143964434,0.005725421560461494,-0.0811094318489313,-0.07453299903133462,0.03323872810288862,0.03586530641258109,0.01583596136175672,-0.1020161053488852,-0.1191610072507027,0.01348568035597609,0.03281495062638147,-0.02915134190144993,0.1621689321962946,-0.06198402103114214,-0.02873636825307706,-0.058584318698116,0.1616556418540435,-0.07051418174188284,0.020645105157005,0.008492680485492716,0.02565352083285731,-0.09109103786597224,-0.04553170911250411,-0.01405662587114815,0.05989923534167463,-0.0004390002330810848,-0.03395008098662636,-0.03458670039558624,-0.08116291767034162,-0.03132855111159179,-0.006759388935384246,-0.1570200195256425,0.05693596847122941,-0.01280061331207964,0.1104733977191608,-0.01442307362408974,-0.01461856738344271,-0.03440223808271363,-0.1095123198330071,-0.06133481340114608,0.1386862679350349,0.006282897539264547,0.003906156973784649,-0.06641448746091086,-0.0181436940837419,-0.04284780236192779,0.1296823248440221,-0.002795468993548998,0.05245486691453299,0.02947734699709484,0.02473676389620565,-0.005191469556376818,-0.1008833980534323,0.02039213395084864,0.0682909654649328,-0.07282960274979138,-0.04379650424617373,0.003889574347985615,0.04740575374380152,0.03690645909799369,-0.04954092738668309,-0.04201479965951715,-0.1142560016388473,-0.03166776743145929,0.06224612236493814,0.004797388376821114,0.0002651681815732944,0.00609879627277348,-0.01771867508963818,-0.05123740554761502,0.0226393461789075,-0.05307699369203973,-0.01950214738003763,0.03929356842896084,-0.04066304413829139,0.06267950863307169,0.0640691348400422,0.1334806568577205,0.002556089188471106,-0.04170395441437,0.09456627330305846,0.1435420709351047,-0.111600804841319,0.01193585674283742,-0.05938868965719699,-0.009562634909841411,-0.01830578060851172,0.02805639598272045,0.01264669753680019,0.006116126440379468,-0.01491362467089191,-0.07659395135305079,-0.08192200950527735,-0.02451111466411191,-0.02679101409243757,0.1291396384163795,0.06211802541587151,0.1051471986430283,0.05213131006547161,-0.001397813615976575,-0.0886033615012794,0.07920904213371144,0.09238536069830872,0.01276669913320457,0.09946979931608975,0.0578110241360688,0.03152795344921468,-0.1007765023532776,0.0002270249478635858,-0.03272492195175378,0.01306141477224247,-0.03342780287225937,0.04295017554702461,0.05698982781806357,-0.2677870686564224,-0.3028370817682299,-0.1980800402811179,-0.3513982596060881,-0.2818018365987821,-0.2738425271331616,-0.1523099073658683,-0.2154631392059148,-0.3057246266285608,-0.2515480629363857,-0.3552839204832801,-0.2976898670206221,-0.3090721710968417,-0.1115059117119033,-0.273676531596576,-0.3959346685588045,-0.3488634136703269,-0.2311098994729672,-0.1865343180300977,-0.2408364889708736,-0.3233421362330461,-0.3012275666114643,-0.1855170371256036,-0.325474815160344,-0.3233109300889992,-0.2459962716298514,-0.3603405608617546,-0.2896361478446621,-0.3274599598071514,-0.3967251473721446,-0.2852748408287364,-0.4124408366363153,-0.2853179623498221,-0.2451613466568569,-0.2927354514699723,-0.30129999500599,-0.2953607337148113,-0.4479309935101792,-0.3107768484955405,-0.2896008458738626,-0.2564130167154912,-0.2776388849386964,-0.2088308083325134,-0.3478972678303933,-0.258386271119144,-0.3745601946413517,-0.2539669026888979,-0.4038219098884953,-0.4320675659829009,-0.2893883500063285,-0.1844978631695635,-0.3558772797985187,-0.2940068493947119,-0.4303394031561767,-0.2999258247542235,-0.3395781574480471,-0.2710678392379925,-0.3133392203167814,-0.396177902682275,-0.3385728903815534,-0.3422661310191032,-0.2700072404134925,-0.2792305037680944,-0.3419318851274665,-0.2850491853019236,-0.2163840167832093,-0.4398850017441265,-0.2222314765338192,-0.4319402668002306,-0.3928045053357193,-0.3744588123453248,-0.3436473166793466,-0.1763468468853556,-0.2258144019414138,-0.3976503952469707,-0.3355459020794305,-0.4112304527392563,-0.328187982798391,-0.2743658598667985,-0.208721289126287,-0.2786269519624789,-0.1779236767098098,-0.2768098941929092,-0.1847690002915767,-0.2598284735355955,-0.1873505433839862,-0.3327852949609197,-0.2122494711127483,-0.2905919987128081,-0.2887562520146499,-0.2674419977971416,-0.2730184844927166,-0.2162592891712297,-0.2137854514467166,-0.3581611340951303,-0.2725075562849464,-0.3001805892075867,-0.302406094635256,-0.2016438456242527,-0.2525941729612058,-0.2666098504571082,-0.2278051732433841,-0.4369560357479058,-0.3805293622107175,-0.2030374285911195,-0.3804708499151095,-0.4192033026580121,-0.4056700606338343,-0.3310572358038575,-0.3118678083656686,-0.4371935026529801,-0.2755752292559581,-0.3009854212870133,-0.3054663510476004,-0.2765161392588739,-0.4187055496690427,-0.3141159685518449,-0.2463675954297977,-0.2536047341115481,-0.2733041148967603,-0.2246640106770554,-0.2791798934394636,-0.2241491845867158,-0.2668367588012506,-0.2917671084076219,-0.2853013203625289,-0.1976795344684701,-0.3095731607250782,-0.3462510197173634,-0.3772316975351036,-0.2748960974108703,-0.2182178089907282,-0.2706479702087976,-0.2713925340576555,-0.271855246290511,-0.3170871623677689,-0.3023253249768894,-0.2846355998270638,-0.347727836578094,-0.299251007168836,-0.2506897633994143,-0.2979617857379211,-0.3196474305612467,-0.3474603598129336,-0.3622823697400495,-0.2656687536384809,-0.1790076209379412,-0.3684177164205232,-0.2502800250104595,-0.2272390063404747,-0.2601069247079984,-0.2862373286273413,-0.1279995083623564,-0.2387248151956874,-0.2214172206180728,-0.2619516788397361,-0.2965710681236308,-0.3113560430515211,-0.3762493756426372,-0.430512685953389,-0.2888599765115248,-0.3415630255157437,-0.3460895548370004,-0.2974880439452589,-0.2911034772720544,-0.389559954439963,-0.2831836511084045,-0.3499634291526753,-0.3567011040401789,-0.2853445527466802,-0.3774607567918768,-0.2577647632416616,-0.2617375711984469,-0.2640105112243476,-0.2513059763654041,-0.3231395420228468,-0.3494106548382082,-0.2923913044461053,-0.3559638895699764,-0.4178129284403095,-0.2154852138368477,-0.4525201284830065,-0.2598254342673106,-0.2618624446641777,-0.3601653382296502,-0.3209584035689198,-0.236994022378517,-0.2359247204683325,-0.2978987454399418,-0.1692477443707122,-0.4238562432374202,-0.185979200474471,-0.340744069842619,-0.3830720427557925,-0.2993057279065162,-0.2966567625805853,-0.2514742410849632,-0.4046143615679247,-0.3695333017916221,-0.2764320067382092,-0.0291194453637781,-0.00461706284626833,0.0340864145985826,0.01708626001512792,-0.01297729838962327,-0.03127688418751946,0.06528042510960273,-0.1054839621437227,-0.07819625174260142,-0.09595000370890031,0.005860738697300897,0.03199549343220073,0.03688349971180572,0.03381772588850687,-0.05192209496845249,0.01786375624845525,-0.0529767300326069,0.01472412911693718,0.1390376095177663,0.02837745574432499,0.08858128542335522,0.01113534892722524,-0.0142752220093142,0.09338825727219792,0.03998830928006506,0.08653866520270755,0.06968071691450767,0.01147502199792712,-0.002692824455761694,0.07538087836700939,-0.07724166314144393,-0.01222680785145334,0.03851366453497579,-0.02610068213065133,-0.04366660886256489,-0.05767864872915349,-0.02043443583120836,-0.1082874969690927,0.1258645923864895,-0.01980467502660792,0.03401731552069481,0.0662124869250625,0.008839274260379025,0.06136722605287735,0.006972040939734123,-0.02984378848630907,-0.06712848579155037,-0.008332798438635099,0.1014176532638953,-0.0256940332347426,0.0315609515727746,0.0714393105044059,-0.1431758865218451,0.07646858807558998,0.062989576500057,0.01045268568497431,0.1016812573301863,0.03027374599897524,-0.0003934410331518768,-0.03908217393349425,-0.02068387265494528,-0.1356428659215603,0.02357755569510275,0.1567307234082295,0.007860399461303642,0.004414956968914014,0.06864444224069659,-0.01157838651537382,-0.0004152528456669766,0.1085161293966821,0.1309270293606358,-0.07975165672272443,-0.07562799875771478,0.03546490940067079,0.006130693159352941,0.02180229986009417,0.05656834504033707,0.03039998527610329,-0.009603103904103329,0.07058800940789357,0.04007797577965128,0.07186928768061002,-0.001504451456130025,0.08815003938091319,0.06349470952727854,0.08235172269673013,0.01399213049083372,-0.05862941661205642,0.0236994617761976,-0.06864087523803863,-0.07019168147199656,-0.06879450836053143,0.0220708363545111,-0.04635416336572658,-0.04849540781061717,-0.07611038166527628,0.05681943287940312,-0.09718349984690744,-0.06676778954564325,0.118010385603203,0.09592403539647147,-0.07055642932607943,-0.09076493499904517,0.2520537869458033,-0.04280333187737899,0.006110623754317776,0.06244078952280434,0.0523751494147886,-0.0006672875991029762,-0.03193804246980689,-0.006860287852496527,-0.1042884189494645,-0.02941582442465027,-0.08280251561753588,-0.05360569975480621,0.07892774124631952,0.03703612272909418,0.07048068042963573,0.02827144657913896,0.1640707715291888,-0.1168501140231317,0.0405363128136191,-0.01966742282050162,0.1851217630486935,0.06873189382839898,0.02708519337384575,-0.01781793367465349,-0.1258859116760496,-0.06921068652970475,-0.0416095456651384,-0.1216528909222478,0.05419658900099498,0.04790699204262997,-0.02429331323373949,-0.02433717828345336,0.07179075104909539,-0.02335976745502309,0.03915041208692416,0.05850195899572207,0.02827496531413797,0.1103198030356219,-0.0987668822150076,0.02982035646773581,0.119552996606274,-0.03335037893824154,0.02104673641353411,0.1604110858638039,0.09006572996675782,0.05981710589164323,-0.02246318019551093,-0.1007814030080022,-0.01085795569297827,-0.04207729820096976,-0.05928928186960222,-0.04362270445607289,0.08264047471813767,-0.002307210999451974,-0.08463477588662477,-0.0573963318956139,-0.08360195609140002,0.06719798887842546,-0.05400157368327434,-0.07994887929944122,0.0626569236418177,-0.01329806217133596,-0.002726181098609153,0.01822956419746816,0.0791823083019559,-0.03069873329100038,-0.1008151636164343,-0.08734954130886081,0.0502049198286347,-0.07566215086117571,-0.01178291481741698,0.04744946071094137,-0.02725546808368978,0.01002403715295864,0.0674265749493317,0.1032231970222168,0.01414758392759062,-0.005052148759583411,0.01538815588457271,0.01648476745575289,-0.01730593751263542,-0.0190550935725063,0.0007186453258607824,-0.09483019019154608,0.003992032861496931,-0.05708347435654521,-0.04798651531034002,0.04475999751314597,-0.1196292338135359,-0.01422903818357911,-0.04575117310908607,-0.0761440211500784,-0.04414178829582188,-0.006934002780802418,-0.0237100294963915,-0.06762318350894227,-0.08063639503201865 2 | -------------------------------------------------------------------------------- /example/rose/rose.R: -------------------------------------------------------------------------------- 1 | df<-read.csv(file="p133_out.csv", header=T) 2 | 3 | png("rose.png") 4 | 5 | plot(df$m, ylab="y(k)", xlab="Time",type="l",lwd=2,lty=2,col="green") 6 | lines(df$f,col="red",lwd=2) 7 | legend("topright", legend=c("Input signal","Filtered signal"), lty=c(2,1), col=c("green","red")) 8 | 9 | dev.off() 10 | -------------------------------------------------------------------------------- /example/rose/rose.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "encoding/csv" 5 | "fmt" 6 | "os" 7 | "strconv" 8 | 9 | "github.com/konimarti/kalman" 10 | "github.com/konimarti/lti" 11 | "gonum.org/v1/gonum/mat" 12 | ) 13 | 14 | func main() { 15 | 16 | // load data 17 | y := load("p133.csv")[0] 18 | 19 | // prepare output file 20 | file, err := os.Create("p133_out.csv") 21 | if err != nil { 22 | panic(err) 23 | } 24 | defer file.Close() 25 | 26 | fmt.Fprintln(file, "m,f") 27 | 28 | // define LTI system 29 | lti := lti.Discrete{ 30 | Ad: mat.NewDense(1, 1, []float64{1}), 31 | Bd: mat.NewDense(1, 1, nil), 32 | C: mat.NewDense(1, 1, []float64{1}), 33 | D: mat.NewDense(1, 1, nil), 34 | } 35 | 36 | // system noise / process model covariance matrix ("Systemrauschen") 37 | Gd := mat.NewDense(1, 1, []float64{1}) 38 | 39 | ctx := kalman.Context{ 40 | // initial state 41 | X: mat.NewVecDense(1, []float64{y[0]}), 42 | // initial covariance matrix 43 | P: mat.NewDense(1, 1, []float64{0}), 44 | } 45 | 46 | // create ROSE filter 47 | gammaR := 9.0 48 | alphaR := 0.5 49 | alphaM := 0.3 50 | filter := kalman.NewRoseFilter(lti, Gd, gammaR, alphaR, alphaM) 51 | 52 | // no control 53 | u := mat.NewVecDense(1, nil) 54 | 55 | for _, row := range y { 56 | // new measurement 57 | y := mat.NewVecDense(1, []float64{row}) 58 | 59 | // apply filter 60 | filter.Apply(&ctx, y, u) 61 | 62 | // get corrected state vector 63 | state := filter.State() 64 | 65 | // print out input and output signals 66 | fmt.Fprintf(file, "%3.8f,%3.8f\n", y.AtVec(0), state.AtVec(0)) 67 | } 68 | } 69 | 70 | func load(file string) [][]float64 { 71 | f, err := os.Open(file) 72 | if err != nil { 73 | panic("could not open file") 74 | } 75 | defer f.Close() 76 | 77 | r := csv.NewReader(f) 78 | r.Comma = ',' 79 | 80 | lines, err := r.ReadAll() 81 | if err != nil { 82 | panic("could read data from file") 83 | } 84 | 85 | y := make([][]float64, len(lines)) 86 | for row, line := range lines { 87 | tmp := make([]float64, len(line)) 88 | for i, entry := range line { 89 | var value float64 90 | if value, err = strconv.ParseFloat(entry, 64); err != nil { 91 | fmt.Println("error parsing", line, i, entry) 92 | panic("could not parse all values") 93 | } 94 | tmp[i] = value 95 | } 96 | y[row] = tmp 97 | } 98 | return y 99 | } 100 | -------------------------------------------------------------------------------- /example/rose/rose.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/konimarti/kalman/25097b2d79d4a6ded52722bbfef9350957bcfaaf/example/rose/rose.png -------------------------------------------------------------------------------- /go.mod: -------------------------------------------------------------------------------- 1 | module github.com/konimarti/kalman 2 | 3 | go 1.14 4 | 5 | require ( 6 | github.com/konimarti/lti v0.0.2-0.20220325211232-ca10963452dd 7 | gonum.org/v1/gonum v0.9.3 8 | gonum.org/v1/netlib v0.0.0-20190331212654-76723241ea4e // indirect 9 | ) 10 | -------------------------------------------------------------------------------- /go.sum: -------------------------------------------------------------------------------- 1 | dmitri.shuralyov.com/gpu/mtl v0.0.0-20190408044501-666a987793e9/go.mod h1:H6x//7gZCb22OMCxBHrMx7a5I7Hp++hsVxbQ4BYO7hU= 2 | gioui.org v0.0.0-20210308172011-57750fc8a0a6/go.mod h1:RSH6KIUZ0p2xy5zHDxgAM4zumjgTw83q2ge/PI+yyw8= 3 | github.com/BurntSushi/xgb v0.0.0-20160522181843-27f122750802/go.mod h1:IVnqGOEym/WlBOVXweHU+Q+/VP0lqqI8lqeDx9IjBqo= 4 | github.com/ajstarks/svgo v0.0.0-20180226025133-644b8db467af/go.mod h1:K08gAheRH3/J6wwsYMMT4xOr94bZjxIelGM0+d/wbFw= 5 | github.com/boombuler/barcode v1.0.0/go.mod h1:paBWMcWSl3LHKBqUq+rly7CNSldXjb2rDl3JlRe0mD8= 6 | github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= 7 | github.com/fogleman/gg v1.2.1-0.20190220221249-0403632d5b90/go.mod h1:R/bRT+9gY/C5z7JzPU0zXsXHKM4/ayA+zqcVNZzPa1k= 8 | github.com/fogleman/gg v1.3.0/go.mod h1:R/bRT+9gY/C5z7JzPU0zXsXHKM4/ayA+zqcVNZzPa1k= 9 | github.com/go-fonts/dejavu v0.1.0/go.mod h1:4Wt4I4OU2Nq9asgDCteaAaWZOV24E+0/Pwo0gppep4g= 10 | github.com/go-fonts/latin-modern v0.2.0/go.mod h1:rQVLdDMK+mK1xscDwsqM5J8U2jrRa3T0ecnM9pNujks= 11 | github.com/go-fonts/liberation v0.1.1/go.mod h1:K6qoJYypsmfVjWg8KOVDQhLc8UDgIK2HYqyqAO9z7GY= 12 | github.com/go-fonts/stix v0.1.0/go.mod h1:w/c1f0ldAUlJmLBvlbkvVXLAD+tAMqobIIQpmnUIzUY= 13 | github.com/go-gl/glfw v0.0.0-20190409004039-e6da0acd62b1/go.mod h1:vR7hzQXu2zJy9AVAgeJqvqgH9Q5CA+iKCZ2gyEVpxRU= 14 | github.com/go-latex/latex v0.0.0-20210118124228-b3d85cf34e07/go.mod h1:CO1AlKB2CSIqUrmQPqA0gdRIlnLEY0gK5JGjh37zN5U= 15 | github.com/golang/freetype v0.0.0-20170609003504-e2365dfdc4a0/go.mod h1:E/TSTwGwJL78qG/PmXZO1EjYhfJinVAhrmmHX6Z8B9k= 16 | github.com/jung-kurt/gofpdf v1.0.0/go.mod h1:7Id9E/uU8ce6rXgefFLlgrJj/GYY22cpxn+r32jIOes= 17 | github.com/jung-kurt/gofpdf v1.0.3-0.20190309125859-24315acbbda5/go.mod h1:7Id9E/uU8ce6rXgefFLlgrJj/GYY22cpxn+r32jIOes= 18 | github.com/konimarti/lti v0.0.2-0.20220325211232-ca10963452dd h1:2vNJXLE1arxEfe0Nr0T+WWI0YqFG9usV86FfXhDnRTs= 19 | github.com/konimarti/lti v0.0.2-0.20220325211232-ca10963452dd/go.mod h1:7BC7AsovAhtoKzYeB2jhqWZzMEEB45zjlFvQ4WDhj9s= 20 | github.com/phpdave11/gofpdf v1.4.2/go.mod h1:zpO6xFn9yxo3YLyMvW8HcKWVdbNqgIfOOp2dXMnm1mY= 21 | github.com/phpdave11/gofpdi v1.0.12/go.mod h1:vBmVV0Do6hSBHC8uKUQ71JGW+ZGQq74llk/7bXwjDoI= 22 | github.com/pkg/errors v0.8.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= 23 | github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= 24 | github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= 25 | github.com/remyoudompheng/bigfft v0.0.0-20170806203942-52369c62f446/go.mod h1:uYEyJGbgTkfkS4+E/PavXkNJcbFIpEtjt2B0KDQ5+9M= 26 | github.com/ruudk/golang-pdf417 v0.0.0-20181029194003-1af4ab5afa58/go.mod h1:6lfFZQK844Gfx8o5WFuvpxWRwnSoipWe/p622j1v06w= 27 | github.com/stretchr/testify v1.2.2/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= 28 | golang.org/x/crypto v0.0.0-20190308221718-c2843e01d9a2/go.mod h1:djNgcEr1/C05ACkg1iLfiJU5Ep61QUkGW8qpdssI0+w= 29 | golang.org/x/crypto v0.0.0-20190510104115-cbcb75029529/go.mod h1:yigFU9vqHzYiE8UmvKecakEJjdnWj3jj499lnFckfCI= 30 | golang.org/x/exp v0.0.0-20180321215751-8460e604b9de/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= 31 | golang.org/x/exp v0.0.0-20180807140117-3d87b88a115f/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= 32 | golang.org/x/exp v0.0.0-20190125153040-c74c464bbbf2/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= 33 | golang.org/x/exp v0.0.0-20190306152737-a1d7652674e8/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= 34 | golang.org/x/exp v0.0.0-20190312203227-4b39c73a6495/go.mod h1:ZjyILWgesfNpC6sMxTJOJm9Kp84zZh5NQWvqDGG3Qr8= 35 | golang.org/x/exp v0.0.0-20191002040644-a1355ae1e2c3 h1:n9HxLrNxWWtEb1cA950nuEEj3QnKbtsCJ6KjcgisNUs= 36 | golang.org/x/exp v0.0.0-20191002040644-a1355ae1e2c3/go.mod h1:NOZ3BPKG0ec/BKJQgnvsSFpcKLM5xXVWnvZS97DWHgE= 37 | golang.org/x/image v0.0.0-20180708004352-c73c2afc3b81/go.mod h1:ux5Hcp/YLpHSI86hEcLt0YII63i6oz57MZXIpbrjZUs= 38 | golang.org/x/image v0.0.0-20190227222117-0694c2d4d067/go.mod h1:kZ7UVZpmo3dzQBMxlp+ypCbDeSB+sBbTgSJuh5dn5js= 39 | golang.org/x/image v0.0.0-20190802002840-cff245a6509b/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 40 | golang.org/x/image v0.0.0-20190910094157-69e4b8554b2a/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 41 | golang.org/x/image v0.0.0-20200119044424-58c23975cae1/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 42 | golang.org/x/image v0.0.0-20200430140353-33d19683fad8/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 43 | golang.org/x/image v0.0.0-20200618115811-c13761719519/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 44 | golang.org/x/image v0.0.0-20201208152932-35266b937fa6/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 45 | golang.org/x/image v0.0.0-20210216034530-4410531fe030/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= 46 | golang.org/x/mobile v0.0.0-20190312151609-d3739f865fa6/go.mod h1:z+o9i4GpDbdi3rU15maQ/Ox0txvL9dWGYEHz965HBQE= 47 | golang.org/x/mobile v0.0.0-20190719004257-d2bd2a29d028/go.mod h1:E/iHnbuqvinMTCcRqshq8CkpyQDoeVncDDYHnLhea+o= 48 | golang.org/x/mod v0.1.0/go.mod h1:0QHyrYULN0/3qlju5TqG8bIK38QM8yzMo5ekMj3DlcY= 49 | golang.org/x/net v0.0.0-20190311183353-d8887717615a/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= 50 | golang.org/x/net v0.0.0-20190404232315-eb5bcb51f2a3/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= 51 | golang.org/x/net v0.0.0-20190620200207-3b0461eec859/go.mod h1:z5CRVTTTmAJ677TzLLGU+0bjPO0LkuOLi4/5GtJWs/s= 52 | golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= 53 | golang.org/x/sys v0.0.0-20190215142949-d0b11bdaac8a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= 54 | golang.org/x/sys v0.0.0-20190312061237-fead79001313/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= 55 | golang.org/x/sys v0.0.0-20190412213103-97732733099d/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= 56 | golang.org/x/sys v0.0.0-20210304124612-50617c2ba197/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= 57 | golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= 58 | golang.org/x/text v0.3.5/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= 59 | golang.org/x/tools v0.0.0-20180525024113-a5b4c53f6e8b/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= 60 | golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= 61 | golang.org/x/tools v0.0.0-20190206041539-40960b6deb8e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= 62 | golang.org/x/tools v0.0.0-20190312151545-0bb0c0a6e846/go.mod h1:LCzVGOaR6xXOjkQ3onu1FJEFr0SW1gC7cKk1uF8kGRs= 63 | golang.org/x/tools v0.0.0-20190927191325-030b2cf1153e/go.mod h1:b+2E5dAYhXwXZwtnZ6UAqBI28+e2cm9otk0dWdXHAEo= 64 | golang.org/x/xerrors v0.0.0-20190717185122-a985d3407aa7/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= 65 | gonum.org/v1/gonum v0.0.0-20180816165407-929014505bf4/go.mod h1:Y+Yx5eoAFn32cQvJDxZx5Dpnq+c3wtXuadVZAcxbbBo= 66 | gonum.org/v1/gonum v0.0.0-20190331200053-3d26580ed485/go.mod h1:2ltnJ7xHfj0zHS40VVPYEAAMTa3ZGguvHGBSJeRWqE0= 67 | gonum.org/v1/gonum v0.8.2/go.mod h1:oe/vMfY3deqTw+1EZJhuvEW2iwGF1bW9wwu7XCu0+v0= 68 | gonum.org/v1/gonum v0.9.3 h1:DnoIG+QAMaF5NvxnGe/oKsgKcAc6PcUyl8q0VetfQ8s= 69 | gonum.org/v1/gonum v0.9.3/go.mod h1:TZumC3NeyVQskjXqmyWt4S3bINhy7B4eYwW69EbyX+0= 70 | gonum.org/v1/netlib v0.0.0-20190313105609-8cb42192e0e0/go.mod h1:wa6Ws7BG/ESfp6dHfk7C6KdzKA7wR7u/rKwOGE66zvw= 71 | gonum.org/v1/netlib v0.0.0-20190331212654-76723241ea4e h1:jRyg0XfpwWlhEV8mDfdNGBeSJM2fuyh9Yjrnd8kF2Ts= 72 | gonum.org/v1/netlib v0.0.0-20190331212654-76723241ea4e/go.mod h1:kS+toOQn6AQKjmKJ7gzohV1XkqsFehRA2FbsbkopSuQ= 73 | gonum.org/v1/plot v0.0.0-20190515093506-e2840ee46a6b/go.mod h1:Wt8AAjI+ypCyYX3nZBvf6cAIx93T+c/OS2HFAYskSZc= 74 | gonum.org/v1/plot v0.9.0/go.mod h1:3Pcqqmp6RHvJI72kgb8fThyUnav364FOsdDo2aGW5lY= 75 | modernc.org/cc v1.0.0/go.mod h1:1Sk4//wdnYJiUIxnW8ddKpaOJCF37yAdqYnkxUpaYxw= 76 | modernc.org/golex v1.0.0/go.mod h1:b/QX9oBD/LhixY6NDh+IdGv17hgB+51fET1i2kPSmvk= 77 | modernc.org/mathutil v1.0.0/go.mod h1:wU0vUrJsVWBZ4P6e7xtFJEhFSNsfRLJ8H458uRjg03k= 78 | modernc.org/strutil v1.0.0/go.mod h1:lstksw84oURvj9y3tn8lGvRxyRC1S2+g5uuIzNfIOBs= 79 | modernc.org/xc v1.0.0/go.mod h1:mRNCo0bvLjGhHO9WsyuKVU4q0ceiDDDoEeWDJHrNx8I= 80 | rsc.io/pdf v0.1.1/go.mod h1:n8OzWcQ6Sp37PL01nO98y4iUCRdTGarVfzxY20ICaU4= 81 | -------------------------------------------------------------------------------- /kalman.go: -------------------------------------------------------------------------------- 1 | package kalman 2 | 3 | // https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ 4 | 5 | import ( 6 | "fmt" 7 | 8 | "github.com/konimarti/lti" 9 | "gonum.org/v1/gonum/mat" 10 | ) 11 | 12 | //Context contains the current state and covariance of the system 13 | type Context struct { 14 | X *mat.VecDense // Current system state 15 | P *mat.Dense // Current covariance matrix 16 | pmt mat.Dense // Workspace for calculating next covariance 17 | mpmt mat.Dense // Workspace for calculating next covariance 18 | k, kt, pct, cpct, denom mat.Dense // Workspace for calculating Update() 19 | cxk, dCtrl, bracket, kupd mat.VecDense 20 | kcp mat.Dense 21 | } 22 | 23 | //Noise represents the measurement and system noise 24 | type Noise struct { 25 | Q *mat.Dense // (discretized) system noise 26 | R *mat.Dense // measurement noise 27 | } 28 | 29 | //NewZeroNoise initializes a Noise struct 30 | //q: dimension of square matrix Q 31 | //r: dimension of square matrix R 32 | func NewZeroNoise(q, r int) Noise { 33 | nse := Noise{ 34 | Q: mat.NewDense(q, q, nil), 35 | R: mat.NewDense(r, r, nil), 36 | } 37 | return nse 38 | } 39 | 40 | //Filter interface for using the Kalman filter 41 | type Filter interface { 42 | Apply(ctx *Context, z, ctrl *mat.VecDense) mat.Vector 43 | State() mat.Vector 44 | } 45 | 46 | //filtImpl is the implementation of the filter interface 47 | type filterImpl struct { 48 | Lti lti.Discrete 49 | Nse Noise 50 | savedState *mat.VecDense 51 | } 52 | 53 | //NewFilter returns a Kalman filter 54 | func NewFilter(lti lti.Discrete, nse Noise) Filter { 55 | return &filterImpl{lti, nse, nil} 56 | } 57 | 58 | //Apply implements the Filter interface 59 | func (f *filterImpl) Apply(ctx *Context, z, ctrl *mat.VecDense) mat.Vector { 60 | // correct state and covariance 61 | err := f.Update(ctx, z, ctrl) 62 | if err != nil { 63 | fmt.Println(err) 64 | } 65 | 66 | // get response of system y 67 | filtered := f.Lti.Response(ctx.X, ctrl) 68 | 69 | // save current context state 70 | f.savedState = mat.VecDenseCopyOf(ctx.X) 71 | 72 | // predict new state 73 | f.NextState(ctx, ctrl) 74 | 75 | // predict new covariance matrix 76 | f.NextCovariance(ctx) 77 | 78 | return filtered 79 | } 80 | 81 | //NextState 82 | func (f *filterImpl) NextState(ctx *Context, ctrl *mat.VecDense) error { 83 | // X_k = Ad * X_k-1 + Bd * ctrl 84 | ctx.X = f.Lti.Predict(ctx.X, ctrl) 85 | return nil 86 | } 87 | 88 | //NextCovariance 89 | func (f *filterImpl) NextCovariance(ctx *Context) error { 90 | // P_new = Ad * P * Ad^t + Q 91 | //ctx.P.Product(f.Lti.Ad, ctx.P, f.Lti.Ad.T()) 92 | //ctx.P.Add(ctx.P, f.Nse.Q) 93 | ctx.P = lti.NewCovariance(f.Lti.Ad).Predict(ctx.P, f.Nse.Q, &ctx.pmt, &ctx.mpmt) 94 | return nil 95 | } 96 | 97 | //Update performs Kalman update 98 | func (f *filterImpl) Update(ctx *Context, z, ctrl mat.Vector) error { 99 | // kalman gain 100 | // K = P C^T (C P C^T + R)^-1 101 | ctx.pct.Mul(ctx.P, f.Lti.C.T()) 102 | ctx.cpct.Mul(f.Lti.C, &ctx.pct) 103 | ctx.denom.Add(&ctx.cpct, f.Nse.R) 104 | 105 | // calculation of Kalman gain with mat.Solve(..) 106 | // K = P C^T (C P C^T + R)^-1 107 | // K * (C P C^T + R) = P C^T 108 | // (C P C^T + R)^T K^T = (P C^T )^T 109 | err := ctx.kt.Solve(ctx.denom.T(), ctx.pct.T()) 110 | if err != nil { 111 | //log.Println(err) 112 | //log.Println("setting Kalman gain to zero") 113 | ctx.denom.Zero() 114 | ctx.k.Product(ctx.P, f.Lti.C.T(), &ctx.denom) 115 | } else { 116 | r, c := ctx.k.Dims() 117 | if r == 0 && c == 0 { 118 | ctx.k.CloneFrom(ctx.kt.T()) 119 | } else { 120 | ctx.k.Copy(ctx.kt.T()) 121 | } 122 | } 123 | 124 | // update state 125 | // X~_k = X_k + K * [z_k - C * X_k - D * ctrl ] 126 | ctx.cxk.MulVec(f.Lti.C, ctx.X) 127 | ctx.dCtrl.MulVec(f.Lti.D, ctrl) 128 | ctx.bracket.SubVec(z, &ctx.cxk) 129 | ctx.bracket.SubVec(&ctx.bracket, &ctx.dCtrl) 130 | ctx.kupd.MulVec(&ctx.k, &ctx.bracket) 131 | ctx.X.AddVec(ctx.X, &ctx.kupd) 132 | 133 | // update covariance 134 | // P~_k = P_k - K * [C * P_k] 135 | ctx.kcp.Product(&ctx.k, f.Lti.C, ctx.P) 136 | ctx.P.Sub(ctx.P, &ctx.kcp) 137 | 138 | return nil 139 | } 140 | 141 | //State return the current state of the context 142 | func (f *filterImpl) State() mat.Vector { 143 | var state mat.VecDense 144 | state.CloneFromVec(f.savedState) 145 | return &state 146 | } 147 | -------------------------------------------------------------------------------- /kalman_test.go: -------------------------------------------------------------------------------- 1 | package kalman 2 | 3 | import ( 4 | "fmt" 5 | "testing" 6 | 7 | "github.com/konimarti/lti" 8 | "gonum.org/v1/gonum/mat" 9 | ) 10 | 11 | // Testing based on example on page 145 in book "Kalman Filter" by R. Marchthaler, 2017 12 | 13 | //newContext 14 | func newContext() *Context { 15 | // define current context 16 | ctx := Context{ 17 | X: mat.NewVecDense(4, []float64{976.32452, 0, 0.092222, 0}), 18 | P: mat.NewDense(4, 4, []float64{ 19 | 3, 0, 0, 0, 20 | 0, 3, 0, 0, 21 | 0, 0, 3, 0, 22 | 0, 0, 0, 0.03, 23 | }), 24 | } 25 | return &ctx 26 | } 27 | 28 | //newSetup is a helper functions for tests 29 | func newSetup() (lti.Discrete, Noise) { 30 | 31 | // define LTI system 32 | dt := 0.1 33 | lti := lti.Discrete{ 34 | Ad: mat.NewDense(4, 4, []float64{ 35 | 1, dt, 0.5 * dt * dt, 0, 36 | 0, 1, dt, 0, 37 | 0, 0, 1, 0, 38 | 0, 0, 0, 1, 39 | }), 40 | Bd: mat.NewDense(4, 1, nil), 41 | C: mat.NewDense(2, 4, []float64{ 42 | 1, 0, 0, 0, 43 | 0, 0, 1, -1, 44 | }), 45 | D: mat.NewDense(2, 1, nil), 46 | } 47 | 48 | // define system and measurement noise 49 | q1 := 100.0 / 9.0 50 | q2 := 0.04 / 1000.0 51 | nse := Noise{ 52 | Q: mat.NewDense(4, 4, []float64{ 53 | 0.25 * q1 * dt * dt * dt * dt, 0.5 * q1 * dt * dt * dt, 0.5 * q1 * dt * dt, 0, 54 | 0.5 * q1 * dt * dt * dt, q1 * dt * dt, q1 * dt, 0, 55 | 0.5 * q1 * dt * dt, q1 * dt, q1, 0, 56 | 0, 0, 0, q2, 57 | }), 58 | R: mat.NewDense(2, 2, []float64{20, 0, 0, 0.2}), 59 | } 60 | 61 | return lti, nse 62 | } 63 | 64 | //NewImplementedFilter returns the implementation of the Kalman filter for testing 65 | func newImplementedFilter() *filterImpl { 66 | lti, nse := newSetup() 67 | return &filterImpl{lti, nse, nil} 68 | } 69 | 70 | func TestPredictionState(t *testing.T) { 71 | ctx := newContext() 72 | filter := newImplementedFilter() 73 | 74 | // predict next state 75 | ctrl := mat.NewVecDense(1, nil) 76 | filter.NextState(ctx, ctrl) 77 | 78 | expectedVec := mat.NewVecDense(4, []float64{ 79 | 976.32498, 0.0092222, 0.092222, 0, 80 | }) 81 | if !mat.EqualApprox(expectedVec, ctx.X, 1e-4) { 82 | fmt.Println("actual:", ctx.X) 83 | fmt.Println("expected:", expectedVec) 84 | t.Error("PredictState") 85 | } 86 | } 87 | 88 | func TestPredictionCovariance(t *testing.T) { 89 | ctx := newContext() 90 | filter := newImplementedFilter() 91 | 92 | // predict next covariance 93 | filter.NextCovariance(ctx) 94 | 95 | // predict next covariance 96 | expected := mat.NewDense(4, 4, []float64{ 97 | 3.0304, 0.30706, 0.070556, 0, 98 | 0.30706, 3.1411, 1.4111, 0, 99 | 0.070556, 1.4111, 14.111, 0, 100 | 0, 0, 0, 0.03004, 101 | }) 102 | if !mat.EqualApprox(expected, ctx.P, 1e-4) { 103 | fmt.Println("actual:", ctx.P) 104 | fmt.Println("expected:", expected) 105 | t.Error("PredictCovariance") 106 | } 107 | } 108 | 109 | func TestUpdate(t *testing.T) { 110 | ctx := newContext() 111 | filter := newImplementedFilter() 112 | 113 | ctrl := mat.NewVecDense(1, nil) 114 | z := mat.NewVecDense(2, []float64{ 115 | 976.32452, 0.092222, 116 | }) 117 | if err := filter.Update(ctx, z, ctrl); err != nil { 118 | t.Error(err) 119 | } 120 | expectedX := mat.NewVecDense(4, []float64{ 121 | 976.32452, 0, 0.092222, 0, 122 | }) 123 | if !mat.EqualApprox(expectedX, ctx.X, 1e-4) { 124 | fmt.Println("actual:", ctx.X) 125 | fmt.Println("expected:", expectedX) 126 | t.Error("UpdateState") 127 | } 128 | } 129 | 130 | func TestFilter(t *testing.T) { 131 | lti, nse := newSetup() 132 | ctx := newContext() 133 | filter := NewFilter(lti, nse) 134 | 135 | ctrl := mat.NewVecDense(1, nil) 136 | 137 | config := []struct { 138 | Iter int 139 | Input []float64 140 | Expected []float64 141 | }{ 142 | { 143 | Iter: 1, 144 | Input: []float64{ 145 | 976.32, 0.092222, 146 | }, 147 | Expected: []float64{ 148 | 976.32452, 0.092222202, 149 | }, 150 | }, 151 | { 152 | Iter: 2, 153 | Input: []float64{ 154 | 979.37006, 0.52210785, 155 | }, 156 | Expected: []float64{ 157 | 976.6817722228133, 0.5147628306401388, 158 | }, 159 | }, 160 | { 161 | Iter: 3, 162 | Input: []float64{ 163 | 977.8754, 0.98211677, 164 | }, 165 | Expected: []float64{ 166 | 976.8229728968552, 0.9740485904798598, 167 | }, 168 | }, 169 | } 170 | 171 | for _, cfg := range config { 172 | z := mat.NewVecDense(2, cfg.Input) 173 | filteredResult := filter.Apply(ctx, z, ctrl) 174 | expectedResult := mat.NewVecDense(2, cfg.Expected) 175 | if !mat.EqualApprox(expectedResult, filteredResult, 1e-4) { 176 | fmt.Println("actual:", filteredResult) 177 | fmt.Println("expected:", expectedResult) 178 | t.Error("ApplyFilter:", cfg.Iter) 179 | } 180 | } 181 | 182 | } 183 | -------------------------------------------------------------------------------- /matlab/Kalman_Filter_Test_Seite_145_Kapitel_10_2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/konimarti/kalman/25097b2d79d4a6ded52722bbfef9350957bcfaaf/matlab/Kalman_Filter_Test_Seite_145_Kapitel_10_2.m -------------------------------------------------------------------------------- /matlab/Rose_Filter_Test_Seite_133_Kapitel_9_4.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/konimarti/kalman/25097b2d79d4a6ded52722bbfef9350957bcfaaf/matlab/Rose_Filter_Test_Seite_133_Kapitel_9_4.m -------------------------------------------------------------------------------- /matlab/Seite_145_Kapitel_10_2_data_t_y.csv: -------------------------------------------------------------------------------- 1 | 1.0000000e-01 9.7632452e+02 9.2222202e-02 2 | 2.0000000e-01 9.7937006e+02 5.2210785e-01 3 | 3.0000000e-01 9.7787540e+02 9.8211677e-01 4 | 4.0000000e-01 9.7039583e+02 6.3782173e-01 5 | 5.0000000e-01 9.7041895e+02 6.3400243e-01 6 | 6.0000000e-01 9.7328137e+02 4.1180123e-01 7 | 7.0000000e-01 9.6953814e+02 4.3451790e-01 8 | 8.0000000e-01 9.7308183e+02 4.5390531e-01 9 | 9.0000000e-01 9.6392866e+02 -7.5174278e-01 10 | 1.0000000e+00 9.6810943e+02 6.7587801e-01 11 | 1.1000000e+00 9.6628951e+02 9.4285199e-01 12 | 1.2000000e+00 9.7022456e+02 -8.0319080e-02 13 | 1.3000000e+00 9.5698273e+02 -1.8064494e-01 14 | 1.4000000e+00 9.6200334e+02 7.7772470e-01 15 | 1.5000000e+00 9.6123924e+02 -1.7418460e-01 16 | 1.6000000e+00 9.4952668e+02 -2.5105147e-01 17 | 1.7000000e+00 9.6458165e+02 8.5284088e-01 18 | 1.8000000e+00 9.7056327e+02 2.0739603e-01 19 | 1.9000000e+00 9.5596164e+02 1.0567376e+00 20 | 2.0000000e+00 9.5728365e+02 -7.7918217e-02 21 | 2.1000000e+00 9.5292223e+02 2.2525322e-01 22 | 2.2000000e+00 9.5571872e+02 6.0808938e-01 23 | 2.3000000e+00 9.5130136e+02 7.4567641e-01 24 | 2.4000000e+00 9.5154483e+02 7.0913242e-01 25 | 2.5000000e+00 9.5823659e+02 5.8562217e-01 26 | 2.6000000e+00 9.4621107e+02 3.9722810e-01 27 | 2.7000000e+00 9.4642457e+02 2.4096635e-01 28 | 2.8000000e+00 9.4766051e+02 7.1487771e-01 29 | 2.9000000e+00 9.4640757e+02 3.2700318e-01 30 | 3.0000000e+00 9.5243679e+02 6.8855220e-01 31 | 3.1000000e+00 9.5408408e+02 -9.0135632e+00 32 | 3.2000000e+00 9.3957535e+02 -1.0505932e+01 33 | 3.3000000e+00 9.3993326e+02 -8.6930119e+00 34 | 3.4000000e+00 9.4387764e+02 -9.7826660e+00 35 | 3.5000000e+00 9.3849746e+02 -8.9112620e+00 36 | 3.6000000e+00 9.4766230e+02 -8.8061069e+00 37 | 3.7000000e+00 9.4451583e+02 -1.0156916e+01 38 | 3.8000000e+00 9.3805001e+02 -9.4208708e+00 39 | 3.9000000e+00 9.3632066e+02 -7.9498414e+00 40 | 4.0000000e+00 9.3043664e+02 -9.5959840e+00 41 | 4.1000000e+00 9.3341513e+02 -9.2825278e+00 42 | 4.2000000e+00 9.3299208e+02 -9.3520237e+00 43 | 4.3000000e+00 9.2643313e+02 -8.9329770e+00 44 | 4.4000000e+00 9.3217288e+02 -9.0123072e+00 45 | 4.5000000e+00 9.2419918e+02 -9.9232656e+00 46 | 4.6000000e+00 9.1825757e+02 -9.6961028e+00 47 | 4.7000000e+00 9.1785899e+02 -9.3465070e+00 48 | 4.8000000e+00 9.1845412e+02 -9.5261188e+00 49 | 4.9000000e+00 9.0901936e+02 -8.3663249e+00 50 | 5.0000000e+00 9.0756780e+02 -9.3038318e+00 51 | 5.1000000e+00 9.1561366e+02 -9.3043361e+00 52 | 5.2000000e+00 8.9983056e+02 -9.8746201e+00 53 | 5.3000000e+00 9.0303216e+02 -1.0084733e+01 54 | 5.4000000e+00 8.9920893e+02 -9.1448486e+00 55 | 5.5000000e+00 8.9930609e+02 -9.5882190e+00 56 | 5.6000000e+00 8.9110240e+02 -9.2096900e+00 57 | 5.7000000e+00 8.7417530e+02 -9.8718466e+00 58 | 5.8000000e+00 8.7773738e+02 -9.0993172e+00 59 | 5.9000000e+00 8.8008855e+02 -1.0311016e+01 60 | 6.0000000e+00 8.7510280e+02 -8.7993579e+00 61 | 6.1000000e+00 8.7877633e+02 -9.1214334e+00 62 | 6.2000000e+00 8.7223923e+02 -9.4507373e+00 63 | 6.3000000e+00 8.6402191e+02 -1.0019260e+01 64 | 6.4000000e+00 8.5792043e+02 -9.6777834e+00 65 | 6.5000000e+00 8.4627500e+02 -9.3862761e+00 66 | 6.6000000e+00 8.5708230e+02 -8.9598601e+00 67 | 6.7000000e+00 8.4479939e+02 -9.9622139e+00 68 | 6.8000000e+00 8.3263607e+02 -8.9207928e+00 69 | 6.9000000e+00 8.3673770e+02 -8.2620577e+00 70 | 7.0000000e+00 8.3041043e+02 -9.7215260e+00 71 | 7.1000000e+00 8.2244933e+02 -9.2903617e+00 72 | 7.2000000e+00 8.2215504e+02 -9.7939521e+00 73 | 7.3000000e+00 8.1163590e+02 -1.0267847e+01 74 | 7.4000000e+00 8.0352145e+02 -8.8423639e+00 75 | 7.5000000e+00 7.9635453e+02 -9.1895724e+00 76 | 7.6000000e+00 7.9712300e+02 -9.7283906e+00 77 | 7.7000000e+00 7.9039555e+02 -9.4492916e+00 78 | 7.8000000e+00 7.8951841e+02 -9.6027729e+00 79 | 7.9000000e+00 7.7507998e+02 -1.0153707e+01 80 | 8.0000000e+00 7.7501874e+02 -1.0789020e+01 81 | 8.1000000e+00 7.6326887e+02 4.7877184e-01 82 | 8.2000000e+00 7.7209341e+02 2.9317894e-01 83 | 8.3000000e+00 7.5747688e+02 2.4211612e-01 84 | 8.4000000e+00 7.5748377e+02 1.2167145e-01 85 | 8.5000000e+00 7.4821345e+02 -3.1269243e-01 86 | 8.6000000e+00 7.4406336e+02 2.6667498e-01 87 | 8.7000000e+00 7.2883818e+02 5.7219632e-01 88 | 8.8000000e+00 7.3362396e+02 2.5143741e-02 89 | 8.9000000e+00 7.2849592e+02 7.0146692e-01 90 | 9.0000000e+00 7.0671222e+02 3.7800099e-01 91 | 9.1000000e+00 7.0921072e+02 4.5462824e-01 92 | 9.2000000e+00 7.0108380e+02 -1.3911561e-01 93 | 9.3000000e+00 6.9870004e+02 1.5814571e-01 94 | 9.4000000e+00 6.8819767e+02 6.8341909e-01 95 | 9.5000000e+00 6.8724793e+02 1.4673134e-01 96 | 9.6000000e+00 6.8267028e+02 5.7228632e-01 97 | 9.7000000e+00 6.7509441e+02 1.3845456e+00 98 | 9.8000000e+00 6.6747074e+02 8.5565396e-01 99 | 9.9000000e+00 6.5623359e+02 9.6392451e-01 100 | 1.0000000e+01 6.5625743e+02 1.5555717e+00 101 | 1.0100000e+01 6.4767032e+02 1.0256527e+00 102 | 1.0200000e+01 6.3828002e+02 6.7787493e-01 103 | 1.0300000e+01 6.3774854e+02 1.3726250e+00 104 | 1.0400000e+01 6.3106228e+02 9.5425375e-02 105 | 1.0500000e+01 6.2685910e+02 1.9493227e-01 106 | 1.0600000e+01 6.2443784e+02 1.4026074e+00 107 | 1.0700000e+01 6.1657704e+02 7.0318845e-01 108 | 1.0800000e+01 6.1383964e+02 4.8117044e-01 109 | 1.0900000e+01 5.9873845e+02 7.0489865e-01 110 | 1.1000000e+01 5.9213350e+02 3.8187115e-01 111 | 1.1100000e+01 5.8957550e+02 3.6942027e-01 112 | 1.1200000e+01 5.9018592e+02 3.2701861e-01 113 | 1.1300000e+01 5.8383024e+02 2.5224683e-01 114 | 1.1400000e+01 5.7143735e+02 7.7952040e-02 115 | 1.1500000e+01 5.6333070e+02 4.7323508e-01 116 | 1.1600000e+01 5.5319946e+02 2.4299316e-01 117 | 1.1700000e+01 5.5700457e+02 1.1448173e+00 118 | 1.1800000e+01 5.4810216e+02 6.0533076e-01 119 | 1.1900000e+01 5.3673277e+02 9.5677348e-01 120 | 1.2000000e+01 5.3450372e+02 4.3746588e-01 121 | 1.2100000e+01 5.2181843e+02 4.6352924e-01 122 | 1.2200000e+01 5.2592297e+02 1.0627051e+00 123 | 1.2300000e+01 5.1722188e+02 1.1567484e+00 124 | 1.2400000e+01 5.1929514e+02 5.1473160e-01 125 | 1.2500000e+01 4.9992045e+02 1.2568992e+00 126 | 1.2600000e+01 5.0434024e+02 7.1275228e-01 127 | 1.2700000e+01 4.9464238e+02 6.6481172e-01 128 | 1.2800000e+01 4.8836941e+02 1.4802925e+00 129 | 1.2900000e+01 4.8520502e+02 1.1867793e+00 130 | 1.3000000e+01 4.8133742e+02 9.0573910e-01 131 | 1.3100000e+01 4.6521007e+02 6.7989595e-01 132 | 1.3200000e+01 4.6989831e+02 2.5922825e-01 133 | 1.3300000e+01 4.5258870e+02 4.4847407e-01 134 | 1.3400000e+01 4.4719641e+02 1.3148232e+00 135 | 1.3500000e+01 4.4299222e+02 -4.0404526e-01 136 | 1.3600000e+01 4.4329356e+02 2.1513502e-01 137 | 1.3700000e+01 4.2921499e+02 4.1556463e-01 138 | 1.3800000e+01 4.2949689e+02 5.6631061e-01 139 | 1.3900000e+01 4.2384199e+02 -2.9777541e-01 140 | 1.4000000e+01 4.1668925e+02 7.3660620e-01 141 | 1.4100000e+01 4.1248012e+02 1.0826346e+01 142 | 1.4200000e+01 3.9880487e+02 1.0079559e+01 143 | 1.4300000e+01 4.0270606e+02 1.0384043e+01 144 | 1.4400000e+01 3.9295768e+02 9.9572863e+00 145 | 1.4500000e+01 3.9006489e+02 1.1226521e+01 146 | 1.4600000e+01 3.7685160e+02 1.0667040e+01 147 | 1.4700000e+01 3.7603269e+02 1.0387798e+01 148 | 1.4800000e+01 3.7437474e+02 1.0086124e+01 149 | 1.4900000e+01 3.6973538e+02 1.0544122e+01 150 | 1.5000000e+01 3.5967278e+02 1.1253233e+01 151 | 1.5100000e+01 3.4671075e+02 1.0623100e+01 152 | 1.5200000e+01 3.4514487e+02 1.0657996e+01 153 | 1.5300000e+01 3.4617773e+02 9.9631757e+00 154 | 1.5400000e+01 3.4400198e+02 1.0574295e+01 155 | 1.5500000e+01 3.3787115e+02 1.0847110e+01 156 | 1.5600000e+01 3.2851024e+02 9.8822033e+00 157 | 1.5700000e+01 3.3683726e+02 1.1004582e+01 158 | 1.5800000e+01 3.2277355e+02 1.1602578e+01 159 | 1.5900000e+01 3.2207070e+02 9.8032782e+00 160 | 1.6000000e+01 3.1219095e+02 9.5757670e+00 161 | 1.6100000e+01 3.0501385e+02 1.0467533e+01 162 | 1.6200000e+01 3.0911454e+02 1.0839485e+01 163 | 1.6300000e+01 3.0684062e+02 1.0463479e+01 164 | 1.6400000e+01 3.0659918e+02 1.0429129e+01 165 | 1.6500000e+01 2.9243429e+02 1.0502906e+01 166 | 1.6600000e+01 2.8396951e+02 1.0023893e+01 167 | 1.6700000e+01 2.9503280e+02 9.7648188e+00 168 | 1.6800000e+01 2.8563126e+02 1.1207291e+01 169 | 1.6900000e+01 2.7615925e+02 1.0630861e+01 170 | 1.7000000e+01 2.7554929e+02 9.9688853e+00 171 | 1.7100000e+01 2.7138273e+02 1.0981174e+01 172 | 1.7200000e+01 2.7724217e+02 9.9197978e+00 173 | 1.7300000e+01 2.7184798e+02 1.0445543e+01 174 | 1.7400000e+01 2.6753560e+02 1.0831283e+01 175 | 1.7500000e+01 2.6487545e+02 1.0124020e+01 176 | 1.7600000e+01 2.5757614e+02 1.0343712e+01 177 | 1.7700000e+01 2.6324133e+02 1.0513252e+01 178 | 1.7800000e+01 2.5799912e+02 1.1373128e+01 179 | 1.7900000e+01 2.5303218e+02 1.0705282e+01 180 | 1.8000000e+01 2.5122394e+02 1.0321422e+01 181 | 1.8100000e+01 2.4954503e+02 3.9920492e-01 182 | 1.8200000e+01 2.4403302e+02 3.7465366e-01 183 | 1.8300000e+01 2.4931554e+02 1.0843850e+00 184 | 1.8400000e+01 2.5022614e+02 3.7282357e-01 185 | 1.8500000e+01 2.5306362e+02 1.4593631e-01 186 | 1.8600000e+01 2.4225360e+02 6.0760804e-01 187 | 1.8700000e+01 2.3432176e+02 4.5767303e-01 188 | 1.8800000e+01 2.3689882e+02 8.5474533e-01 189 | 1.8900000e+01 2.2949632e+02 9.2907440e-01 190 | 1.9000000e+01 2.2925781e+02 9.8616716e-01 191 | 1.9100000e+01 2.2705303e+02 7.8942265e-01 192 | 1.9200000e+01 2.2427369e+02 7.8288504e-01 193 | 1.9300000e+01 2.2769127e+02 1.3511710e+00 194 | 1.9400000e+01 2.1901204e+02 -1.8779191e-02 195 | 1.9500000e+01 2.2498399e+02 3.4467858e-01 196 | 1.9600000e+01 2.2355684e+02 -2.5805596e-01 197 | 1.9700000e+01 2.2263980e+02 9.1116195e-01 198 | 1.9800000e+01 2.1590863e+02 5.0962905e-01 199 | 1.9900000e+01 2.1676804e+02 3.6613093e-01 200 | 2.0000000e+01 2.2006355e+02 -2.6966168e-01 201 | 2.0100000e+01 2.1023699e+02 5.8203323e-01 202 | 2.0200000e+01 2.0720445e+02 3.8384419e-01 203 | 2.0300000e+01 2.1392678e+02 -1.0588004e-01 204 | 2.0400000e+01 1.9278029e+02 -1.7623799e-02 205 | 2.0500000e+01 1.9995807e+02 1.5922934e-01 206 | 2.0600000e+01 2.1086898e+02 3.8806262e-01 207 | 2.0700000e+01 1.9589159e+02 -2.3734927e-01 208 | 2.0800000e+01 1.9349413e+02 4.1861132e-01 209 | 2.0900000e+01 1.8851507e+02 7.7534370e-01 210 | 2.1000000e+01 1.9554618e+02 3.3154614e-01 211 | 2.1100000e+01 1.8056406e+02 1.8782914e-01 212 | 2.1200000e+01 1.8442500e+02 6.3600769e-01 213 | 2.1300000e+01 1.8853559e+02 9.9241791e-01 214 | 2.1400000e+01 1.8399355e+02 3.0027084e-01 215 | 2.1500000e+01 1.8479028e+02 2.9127994e-01 216 | 2.1600000e+01 1.8056546e+02 -1.4211851e-01 217 | 2.1700000e+01 1.8104493e+02 6.2752064e-02 218 | 2.1800000e+01 1.7024250e+02 7.7096430e-01 219 | 2.1900000e+01 1.7497531e+02 4.4917215e-01 220 | 2.2000000e+01 1.6499410e+02 8.4192232e-01 221 | 2.2100000e+01 1.7350294e+02 7.9958725e-01 222 | 2.2200000e+01 1.6661109e+02 6.6805557e-01 223 | 2.2300000e+01 1.6893070e+02 8.4285756e-01 224 | 2.2400000e+01 1.6159637e+02 1.1813173e+00 225 | 2.2500000e+01 1.6346974e+02 7.5796362e-01 226 | 2.2600000e+01 1.6471395e+02 6.9050092e-01 227 | 2.2700000e+01 1.4914652e+02 4.8080069e-01 228 | 2.2800000e+01 1.4764879e+02 -5.5556908e-02 229 | 2.2900000e+01 1.4580546e+02 2.7417875e-01 230 | 2.3000000e+01 1.5579502e+02 6.4572514e-01 231 | 2.3100000e+01 1.5181205e+02 2.9563574e-03 232 | 2.3200000e+01 1.4485852e+02 7.0937839e-01 233 | 2.3300000e+01 1.5270394e+02 6.4427686e-01 234 | 2.3400000e+01 1.4461624e+02 5.4473095e-01 235 | 2.3500000e+01 1.3490352e+02 6.3480564e-01 236 | 2.3600000e+01 1.3544224e+02 5.1065425e-01 237 | 2.3700000e+01 1.3180505e+02 4.9013682e-01 238 | 2.3800000e+01 1.4027038e+02 4.9608334e-01 239 | 2.3900000e+01 1.3131171e+02 9.1566497e-01 240 | 2.4000000e+01 1.2675914e+02 4.5955744e-01 241 | 2.4100000e+01 1.2741908e+02 -6.8115630e-01 242 | 2.4200000e+01 1.2425937e+02 2.8258711e-01 243 | 2.4300000e+01 1.2981460e+02 5.8764028e-01 244 | 2.4400000e+01 1.2613469e+02 9.2920082e-01 245 | 2.4500000e+01 1.2335202e+02 1.1172998e+00 246 | 2.4600000e+01 1.2185941e+02 7.8964661e-02 247 | 2.4700000e+01 1.1819219e+02 8.4027056e-01 248 | 2.4800000e+01 1.1175660e+02 6.0640126e-01 249 | 2.4900000e+01 1.1642215e+02 4.0681256e-01 250 | 2.5000000e+01 1.0832780e+02 5.1106371e-01 251 | 2.5100000e+01 1.0520629e+02 4.8743728e-01 252 | 2.5200000e+01 1.0319652e+02 5.6277051e-01 253 | 2.5300000e+01 1.0988876e+02 6.0625177e-01 254 | 2.5400000e+01 1.0838740e+02 1.9967428e-01 255 | 2.5500000e+01 9.4134294e+01 3.2661784e-02 256 | 2.5600000e+01 9.9337695e+01 9.3190397e-01 257 | 2.5700000e+01 9.9126718e+01 4.0172190e-01 258 | 2.5800000e+01 9.3270277e+01 1.1325997e+00 259 | 2.5900000e+01 9.7403205e+01 8.6691163e-02 260 | 2.6000000e+01 9.3137843e+01 2.3429215e-01 261 | 2.6100000e+01 8.9316507e+01 1.1356137e+00 262 | 2.6200000e+01 8.5507183e+01 1.1295013e+00 263 | 2.6300000e+01 8.9027715e+01 3.9816872e-02 264 | 2.6400000e+01 8.7798174e+01 5.9235508e-01 265 | 2.6500000e+01 8.4098806e+01 1.0997632e+00 266 | 2.6600000e+01 7.8927664e+01 1.0960090e+00 267 | 2.6700000e+01 8.2118772e+01 -7.4622380e-02 268 | 2.6800000e+01 7.1944303e+01 1.2237597e+00 269 | 2.6900000e+01 7.4645958e+01 7.9589609e-01 270 | 2.7000000e+01 7.8244931e+01 6.0167242e-01 271 | 2.7100000e+01 6.9151641e+01 1.0399119e+01 272 | 2.7200000e+01 6.4276251e+01 1.0067995e+01 273 | 2.7300000e+01 7.4891319e+01 1.0542493e+01 274 | 2.7400000e+01 6.3345733e+01 1.0385186e+01 275 | 2.7500000e+01 6.4774981e+01 1.1533122e+01 276 | 2.7600000e+01 5.7656140e+01 1.0584997e+01 277 | 2.7700000e+01 6.1086228e+01 1.0422451e+01 278 | 2.7800000e+01 6.0538568e+01 1.0493726e+01 279 | 2.7900000e+01 5.7220400e+01 1.0225975e+01 280 | 2.8000000e+01 5.7508320e+01 1.1426537e+01 281 | 2.8100000e+01 6.0387076e+01 1.0784886e+01 282 | 2.8200000e+01 4.7770603e+01 1.0533494e+01 283 | 2.8300000e+01 5.4119308e+01 1.1002340e+01 284 | 2.8400000e+01 4.3545102e+01 1.0485185e+01 285 | 2.8500000e+01 5.1927049e+01 1.0456286e+01 286 | 2.8600000e+01 5.9883527e+01 2.5107202e-01 287 | 2.8700000e+01 5.4958892e+01 2.2474048e-01 288 | 2.8800000e+01 5.0219033e+01 1.2175780e+00 289 | 2.8900000e+01 4.8237337e+01 8.4368161e-01 290 | 2.9000000e+01 5.1612476e+01 5.3887783e-01 291 | 2.9100000e+01 5.3476599e+01 1.2622126e+00 292 | 2.9200000e+01 4.8315922e+01 5.1056938e-01 293 | 2.9300000e+01 5.4971432e+01 6.2971109e-01 294 | 2.9400000e+01 4.3601386e+01 -1.3499823e-01 295 | 2.9500000e+01 3.7815750e+01 7.1254880e-01 296 | 2.9600000e+01 4.2903986e+01 -1.4726075e-01 297 | 2.9700000e+01 4.4858685e+01 5.8027751e-02 298 | 2.9800000e+01 4.3582966e+01 9.2457543e-01 299 | 2.9900000e+01 4.0960441e+01 6.5707214e-01 300 | 3.0000000e+01 5.0230310e+01 1.0990382e-01 301 | 3.0100000e+01 3.6521609e+01 3.2195212e-01 302 | 3.0200000e+01 4.1050525e+01 6.1468161e-01 303 | 3.0300000e+01 3.8415152e+01 5.9839837e-01 304 | 3.0400000e+01 4.0129725e+01 -2.6537950e-01 305 | 3.0500000e+01 3.8770825e+01 -3.9230137e-02 306 | 3.0600000e+01 4.3389357e+01 -2.9287109e-01 307 | 3.0700000e+01 3.6094246e+01 4.6758075e-01 308 | 3.0800000e+01 3.6579901e+01 -2.6970596e-01 309 | 3.0900000e+01 4.0202265e+01 8.1389083e-01 310 | 3.1000000e+01 4.4097717e+01 5.0341043e-02 311 | 3.1100000e+01 3.0456302e+01 5.1908829e-03 312 | 3.1200000e+01 4.6298739e+01 1.3007627e+00 313 | 3.1300000e+01 3.3821791e+01 1.4670476e+00 314 | 3.1400000e+01 4.0146749e+01 1.3360796e-01 315 | 3.1500000e+01 4.1443798e+01 4.8343092e-01 316 | 3.1600000e+01 4.2167053e+01 1.3778821e+00 317 | 3.1700000e+01 3.3436300e+01 2.5838553e-01 318 | 3.1800000e+01 3.5358740e+01 1.2680910e+00 319 | 3.1900000e+01 3.4023873e+01 8.6662941e-01 320 | 3.2000000e+01 3.1541401e+01 5.2482967e-01 321 | 3.2100000e+01 3.4965123e+01 3.4207755e-01 322 | 3.2200000e+01 3.4853902e+01 1.2571994e+00 323 | 3.2300000e+01 3.1174399e+01 8.1799141e-01 324 | 3.2400000e+01 3.2841932e+01 2.1701936e-01 325 | 3.2500000e+01 4.3706621e+01 6.7563258e-01 326 | 3.2600000e+01 3.1216163e+01 1.0736365e-01 327 | 3.2700000e+01 3.3926571e+01 5.6661042e-01 328 | 3.2800000e+01 2.8181461e+01 1.1850767e+00 329 | 3.2900000e+01 2.7389258e+01 7.3784955e-01 330 | 3.3000000e+01 2.7703593e+01 1.6026654e-01 331 | 3.3100000e+01 3.1274779e+01 6.5562324e-01 332 | 3.3200000e+01 2.4665408e+01 1.8793315e-01 333 | 3.3300000e+01 1.9463370e+01 1.4030650e+00 334 | 3.3400000e+01 2.7487989e+01 -3.0216682e-01 335 | 3.3500000e+01 2.4798292e+01 2.0505926e-01 336 | 3.3600000e+01 2.7502231e+01 8.4500442e-01 337 | 3.3700000e+01 2.6128369e+01 1.3313231e-01 338 | 3.3800000e+01 2.8510720e+01 5.0799984e-01 339 | 3.3900000e+01 2.8778290e+01 7.9271705e-01 340 | 3.4000000e+01 1.8321565e+01 1.0624600e+00 341 | 3.4100000e+01 1.6931876e+01 8.5397224e-02 342 | 3.4200000e+01 2.5496950e+01 4.2402840e-01 343 | 3.4300000e+01 2.4120224e+01 2.8906165e-01 344 | 3.4400000e+01 1.8049947e+01 4.4891323e-01 345 | 3.4500000e+01 2.7283831e+01 6.6523956e-01 346 | 3.4600000e+01 2.1397634e+01 2.2334684e-01 347 | 3.4700000e+01 2.1302614e+01 8.5782236e-01 348 | 3.4800000e+01 2.1475094e+01 6.9082428e-01 349 | 3.4900000e+01 2.3122732e+01 2.8876726e-01 350 | 3.5000000e+01 2.6067760e+01 6.2340939e-01 351 | 3.5100000e+01 1.7578409e+01 9.4015974e-01 352 | 3.5200000e+01 1.0510148e+01 9.4413115e-01 353 | 3.5300000e+01 2.2282350e+01 7.9883220e-01 354 | 3.5400000e+01 1.3101038e+01 4.7093182e-01 355 | 3.5500000e+01 2.2311907e+01 2.2268346e-01 356 | 3.5600000e+01 1.3613726e+01 6.0022248e-01 357 | 3.5700000e+01 2.2420109e+01 2.9153756e-01 358 | 3.5800000e+01 1.1645556e+01 3.5148623e-01 359 | 3.5900000e+01 9.7743058e+00 9.1358975e-01 360 | 3.6000000e+01 1.8529564e+01 1.1471295e+00 361 | 3.6100000e+01 7.6542480e+00 7.6646422e-01 362 | 3.6200000e+01 1.6293668e+01 1.4182505e+00 363 | 3.6300000e+01 1.4708007e+01 -1.8390154e-01 364 | 3.6400000e+01 1.1609288e+01 5.1014619e-01 365 | 3.6500000e+01 1.0222620e+01 5.4261266e-01 366 | 3.6600000e+01 2.0452809e+01 1.2220381e+00 367 | 3.6700000e+01 9.0750284e+00 7.2417618e-01 368 | 3.6800000e+01 9.3993854e+00 3.5517425e-01 369 | 3.6900000e+01 5.5518610e+00 7.4785320e-01 370 | 3.7000000e+01 5.6120857e+00 7.8730945e-01 371 | 3.7100000e+01 6.1399776e+00 5.8167243e-01 372 | 3.7200000e+01 1.5895230e+01 -4.0671500e-01 373 | 3.7300000e+01 6.0039895e+00 9.5753571e-01 374 | 3.7400000e+01 4.1639339e+00 -1.0614650e+00 375 | 3.7500000e+01 4.7454961e+00 5.4896525e-01 376 | 3.7600000e+01 9.4447882e+00 -1.9377653e-01 377 | 3.7700000e+01 -1.3412902e-01 3.4118957e-01 378 | 3.7800000e+01 8.0545753e+00 1.1254539e+00 379 | 3.7900000e+01 -1.2037315e+00 2.7036702e-01 380 | 3.8000000e+01 3.6648293e+00 1.3573888e+00 381 | 3.8100000e+01 -4.4201281e+00 8.4811579e-01 382 | 3.8200000e+01 5.6467181e+00 3.8978192e-01 383 | 3.8300000e+01 5.8485852e+00 9.5928360e-02 384 | 3.8400000e+01 -8.2481598e-01 2.7822577e-01 385 | 3.8500000e+01 3.1702420e+00 6.6747916e-01 386 | 3.8600000e+01 1.9782531e+00 9.4398658e+00 387 | 3.8700000e+01 5.9837239e+00 1.0775064e+01 388 | 3.8800000e+01 1.0241689e+00 1.0624981e+01 389 | 3.8900000e+01 4.8485981e+00 1.0866208e+01 390 | 3.9000000e+01 -4.7221642e-01 1.1415673e+01 391 | 3.9100000e+01 -2.8992719e+00 3.5521712e-01 392 | 3.9200000e+01 -1.6985711e-01 6.1488156e-02 393 | 3.9300000e+01 -8.9140769e-01 1.0275484e+00 394 | 3.9400000e+01 3.9429475e+00 9.0006957e-01 395 | 3.9500000e+01 -2.5108392e-01 4.3607948e-01 396 | 3.9600000e+01 -3.7621346e+00 2.3459191e-01 397 | 3.9700000e+01 -6.9248270e-01 6.1131916e-01 398 | 3.9800000e+01 5.3155570e+00 -5.2540243e-03 399 | 3.9900000e+01 -1.8555218e+00 5.2955841e-01 400 | 4.0000000e+01 -2.9454237e+00 2.1471145e-01 401 | 4.0100000e+01 4.4005400e+00 1.3359448e-01 402 | 4.0200000e+01 4.5270312e-01 9.3288197e-01 403 | 4.0300000e+01 -1.6220698e+00 5.1357249e-01 404 | 4.0400000e+01 -3.3041385e+00 1.1042287e+00 405 | 4.0500000e+01 -6.6180565e+00 1.4158660e-02 406 | 4.0600000e+01 6.8589932e-01 6.0744979e-01 407 | 4.0700000e+01 -1.4177452e+00 3.2133509e-02 408 | 4.0800000e+01 1.4262458e+00 7.7669894e-01 409 | 4.0900000e+01 -6.5138744e+00 1.0836290e+00 410 | 4.1000000e+01 5.9114236e+00 9.5771913e-01 411 | 4.1100000e+01 2.1788422e+00 -4.4602779e-01 412 | 4.1200000e+01 -7.0061656e+00 8.0487842e-01 413 | 4.1300000e+01 4.7273144e+00 5.0389491e-01 414 | 4.1400000e+01 -2.6277278e+00 6.4949158e-01 415 | 4.1500000e+01 1.3515762e+00 2.5518957e-01 416 | 4.1600000e+01 9.8146564e-01 -2.3834264e-01 417 | 4.1700000e+01 -3.9274347e+00 8.9926929e-01 418 | 4.1800000e+01 -1.2796569e+01 2.7323786e-01 419 | 4.1900000e+01 2.3950761e+00 3.5251291e-01 420 | 4.2000000e+01 -1.5296374e+00 5.7529011e-01 421 | 4.2100000e+01 -2.6737061e+00 1.6344646e+00 422 | 4.2200000e+01 1.8734346e+00 1.5257410e+00 423 | 4.2300000e+01 -3.0843459e+00 -8.9723090e-02 424 | 4.2400000e+01 3.0016576e+00 2.8796050e-01 425 | 4.2500000e+01 6.7483396e-01 9.4157065e-01 426 | 4.2600000e+01 -4.4333128e+00 -4.2708560e-01 427 | 4.2700000e+01 3.7424173e+00 1.0563454e+00 428 | 4.2800000e+01 2.1220686e+00 3.5902057e-01 429 | 4.2900000e+01 5.5998559e+00 1.1257833e+00 430 | 4.3000000e+01 -3.9919299e+00 1.0650508e-01 431 | 4.3100000e+01 -4.0037881e+00 8.7457776e-02 432 | 4.3200000e+01 1.4012361e+00 1.5350324e+00 433 | 4.3300000e+01 2.9830630e+00 7.6296750e-01 434 | 4.3400000e+01 3.7034498e+00 3.6146770e-01 435 | 4.3500000e+01 2.9110377e-02 3.8044390e-02 436 | 4.3600000e+01 -1.0549420e+00 9.9343345e-01 437 | 4.3700000e+01 2.9195047e+00 6.5299576e-01 438 | 4.3800000e+01 8.7856565e+00 -5.1248855e-01 439 | 4.3900000e+01 3.9559807e+00 9.6038302e-02 440 | 4.4000000e+01 3.7806077e-01 1.8282373e+00 441 | 4.4100000e+01 -2.5693050e+00 4.9473795e-01 442 | 4.4200000e+01 2.2349528e+00 -3.9422106e-01 443 | 4.4300000e+01 -2.1649811e+00 1.0286748e-01 444 | 4.4400000e+01 1.0663613e+00 3.1221394e-01 445 | 4.4500000e+01 3.4804317e+00 8.1062488e-01 446 | 4.4600000e+01 4.1334742e+00 4.2721478e-01 447 | 4.4700000e+01 2.6302327e+00 5.4542646e-01 448 | 4.4800000e+01 6.1623094e+00 4.9201345e-01 449 | 4.4900000e+01 8.2788401e+00 7.0528668e-02 450 | 4.5000000e+01 -8.4866148e+00 1.0244946e+00 451 | 4.5100000e+01 -7.9545175e+00 3.3664509e-01 452 | 4.5200000e+01 -4.1261770e+00 2.9275646e-01 453 | 4.5300000e+01 -8.9350015e+00 5.8720199e-01 454 | 4.5400000e+01 -1.5970000e+00 4.7006061e-01 455 | 4.5500000e+01 -1.5044299e+00 2.6676463e-01 456 | 4.5600000e+01 1.1199391e+00 5.5684074e-01 457 | 4.5700000e+01 1.2797549e+00 1.4449706e+00 458 | 4.5800000e+01 -3.0686391e+00 7.4206827e-01 459 | 4.5900000e+01 -3.6372530e+00 2.0703696e-01 460 | 4.6000000e+01 3.5482041e+00 1.1113493e-01 461 | 4.6100000e+01 -1.7081111e+00 3.9316161e-01 462 | 4.6200000e+01 -6.1322115e+00 -2.0480296e-01 463 | 4.6300000e+01 4.6096338e-02 1.3165008e+00 464 | 4.6400000e+01 9.1290626e-01 5.9135119e-01 465 | 4.6500000e+01 -1.8381619e+00 -2.0286415e-02 466 | 4.6600000e+01 2.9675358e+00 5.5393835e-01 467 | 4.6700000e+01 1.0097284e+00 -1.4556104e-01 468 | 4.6800000e+01 -9.5186334e-01 7.6043307e-01 469 | 4.6900000e+01 -1.9692859e-01 4.1884618e-01 470 | 4.7000000e+01 2.0490514e+00 5.1878735e-01 471 | 4.7100000e+01 -1.9740785e+00 6.6182064e-01 472 | 4.7200000e+01 -4.7178652e+00 8.8179279e-01 473 | 4.7300000e+01 -6.9579396e-01 1.3785894e-01 474 | 4.7400000e+01 5.7756680e-01 -1.5608500e-01 475 | 4.7500000e+01 2.2782327e+00 4.5353619e-01 476 | 4.7600000e+01 -1.3471652e-01 1.3198194e-01 477 | 4.7700000e+01 -2.0453982e+00 6.9005747e-01 478 | 4.7800000e+01 2.6668940e+00 -3.2944007e-01 479 | 4.7900000e+01 -5.0771464e-01 8.8396645e-01 480 | 4.8000000e+01 3.6091229e+00 6.3702341e-01 481 | 4.8100000e+01 -4.0161006e-01 2.8944720e-03 482 | 4.8200000e+01 -2.8139239e-02 3.5812564e-01 483 | 4.8300000e+01 -4.1081341e-01 8.7470357e-01 484 | 4.8400000e+01 -4.1199028e+00 1.5851483e+00 485 | 4.8500000e+01 -4.1454533e+00 3.4298803e-01 486 | 4.8600000e+01 -4.2987095e+00 1.1180321e+00 487 | 4.8700000e+01 7.9820017e+00 1.1510318e-01 488 | 4.8800000e+01 -8.9527236e-01 5.9252423e-01 489 | 4.8900000e+01 4.2056488e+00 4.0720441e-01 490 | 4.9000000e+01 1.5614746e+00 6.5216757e-01 491 | 4.9100000e+01 8.3149543e+00 2.7723748e-01 492 | 4.9200000e+01 4.1459919e+00 -1.3566702e-01 493 | 4.9300000e+01 -5.4871491e+00 3.7890612e-01 494 | 4.9400000e+01 -1.4634579e+00 6.9662332e-01 495 | 4.9500000e+01 3.9875639e+00 2.7364562e-01 496 | 4.9600000e+01 1.2888051e+00 4.1755696e-01 497 | 4.9700000e+01 1.0130262e+01 6.7977942e-01 498 | 4.9800000e+01 -2.1417207e-01 7.4115020e-01 499 | 4.9900000e+01 -6.9401471e+00 1.7192714e-01 500 | 5.0000000e+01 1.9860009e+00 3.7998040e-01 501 | -------------------------------------------------------------------------------- /rose.go: -------------------------------------------------------------------------------- 1 | package kalman 2 | 3 | import ( 4 | "github.com/konimarti/lti" 5 | "gonum.org/v1/gonum/mat" 6 | ) 7 | 8 | type roseImpl struct { 9 | Std filterImpl 10 | Rose rose 11 | } 12 | 13 | //Apply implements the Filter interface 14 | func (r *roseImpl) Apply(ctx *Context, z, ctrl *mat.VecDense) mat.Vector { 15 | 16 | // adaptively update noise matrices 17 | r.Std.Nse.R = r.Rose.R(z) 18 | r.Std.Nse.Q = r.Rose.Q(ctx, z, ctrl, r.Std.Lti, r.Std.Nse.R) 19 | 20 | return r.Std.Apply(ctx, z, ctrl) 21 | } 22 | 23 | //State return the current state of the context 24 | func (r *roseImpl) State() mat.Vector { 25 | var state mat.VecDense 26 | state.CloneFromVec(r.Std.savedState) 27 | return &state 28 | } 29 | 30 | type rose struct { 31 | Gamma float64 32 | AlphaR float64 33 | AlphaM float64 34 | 35 | Gd *mat.Dense 36 | 37 | E1 *mat.VecDense 38 | EE1 *mat.Dense 39 | 40 | M *mat.Dense 41 | } 42 | 43 | //Q 44 | func (r *rose) Q(ctx *Context, y, ctrl *mat.VecDense, lti lti.Discrete, R *mat.Dense) *mat.Dense { 45 | x := ctx.X 46 | P := ctx.P 47 | H := lti.C 48 | F := lti.Ad 49 | D := lti.D 50 | 51 | // dy = y - hx - d ctrl 52 | var hx, dctrl, dy mat.VecDense 53 | hx.MulVec(H, x) 54 | dctrl.MulVec(D, ctrl) 55 | dy.SubVec(y, &hx) 56 | dy.SubVec(&dy, &dctrl) 57 | 58 | // M = AlphaM * dy * dy' + (1-AlphaM) * M 59 | var dy2 mat.Dense 60 | dy2.Outer(r.AlphaM, &dy, &dy) 61 | r.M.Scale(1.0-r.AlphaM, r.M) 62 | r.M.Add(&dy2, r.M) 63 | 64 | // hmrh = H^T (M-R) H 65 | var mr, hmrh mat.Dense 66 | mr.Sub(r.M, R) 67 | hmrh.Product(H.T(), &mr, H) 68 | 69 | // fpf = F P F^T 70 | var fpf mat.Dense 71 | fpf.Product(F, P, F.T()) 72 | 73 | // qk = hmrh - fpf = H^T (M-R) H - F P F^T 74 | var qk mat.Dense 75 | qk.Sub(&hmrh, &fpf) 76 | 77 | // q = Gd * qk * Gd^T 78 | var q mat.Dense 79 | q.Product(r.Gd, &qk, r.Gd.T()) 80 | 81 | // make sure there are no negative values 82 | n, c := q.Dims() 83 | for i := 0; i < n; i++ { 84 | for j := 0; j < c; j++ { 85 | if q.At(i, j) < 0.0 { 86 | q.Set(i, j, 0.0) 87 | } 88 | } 89 | } 90 | 91 | return &q 92 | } 93 | 94 | //R returns a matrix with an adaptivly calculated measurement error 95 | func (r *rose) R(y mat.Vector) *mat.Dense { 96 | // E1 = AlphaR * y(k) + (1-AlphaR) * E1 97 | var ay mat.VecDense 98 | ay.ScaleVec(r.AlphaR, y) 99 | r.E1.ScaleVec(1.0-r.AlphaR, r.E1) 100 | r.E1.AddVec(&ay, r.E1) 101 | 102 | // EE1 = AlphaR * (y(k) * y'(k)) + (1-AlphaR) * EE1 103 | var y2 mat.Dense 104 | y2.Outer(r.AlphaR, y, y) 105 | r.EE1.Scale(1.0-r.AlphaR, r.EE1) 106 | r.EE1.Add(&y2, r.EE1) 107 | 108 | // calculate R = Gamma * (EE1 - E1 * E1') 109 | var e1e1, diff, rm mat.Dense 110 | e1e1.Outer(1.0, r.E1, r.E1) 111 | diff.Sub(r.EE1, &e1e1) 112 | rm.Scale(r.Gamma, &diff) 113 | 114 | return &rm 115 | } 116 | 117 | //NewRoseFilter returns a ROSE Kalman filter 118 | //Rapid Ongoing Stochasic covariance Estimation (ROSE) Filter 119 | //lti: discrete linear, time-invariante system 120 | //Gd: discretized G matrix for system noise 121 | //gammaR: Gain factor for measurement noise 122 | //alphaR: Kalman gain for measurment covariance noise 123 | //alphaM: Kalman gain for covariance M 124 | func NewRoseFilter(lti lti.Discrete, Gd *mat.Dense, gammaR, alphaR, alphaM float64) Filter { 125 | // create dummy noise struct 126 | q, _ := lti.Ad.Dims() 127 | r, _ := lti.C.Dims() 128 | nse := NewZeroNoise(q, r) 129 | 130 | // create the standard Kalman filter 131 | filter := filterImpl{lti, nse, nil} 132 | 133 | // create new rose struct 134 | rose := rose{ 135 | Gamma: gammaR, 136 | AlphaR: alphaR, 137 | AlphaM: alphaM, 138 | Gd: Gd, 139 | E1: mat.NewVecDense(r, nil), 140 | EE1: mat.NewDense(r, r, nil), 141 | M: mat.NewDense(r, r, nil), 142 | } 143 | 144 | return &roseImpl{filter, rose} 145 | } 146 | -------------------------------------------------------------------------------- /rose_test.go: -------------------------------------------------------------------------------- 1 | package kalman 2 | 3 | import ( 4 | "fmt" 5 | "testing" 6 | 7 | "github.com/konimarti/lti" 8 | "gonum.org/v1/gonum/mat" 9 | ) 10 | 11 | // Testing based on example on page 145 in book "Kalman Filter" by R. Marchthaler, 2017 12 | 13 | //newContext for Rose Filter 14 | func newRoseContext() *Context { 15 | // define current context 16 | ctx := Context{ 17 | X: mat.NewVecDense(1, []float64{0.04280385872149909}), 18 | P: mat.NewDense(1, 1, []float64{0}), 19 | } 20 | return &ctx 21 | } 22 | 23 | //newRoseFilter is a helper functions for tests 24 | func newRoseFilter() *roseImpl { 25 | 26 | // define LTI system 27 | rose := NewRoseFilter( 28 | lti.Discrete{ 29 | Ad: mat.NewDense(1, 1, []float64{1}), 30 | Bd: mat.NewDense(1, 1, []float64{0}), 31 | C: mat.NewDense(1, 1, []float64{1}), 32 | D: mat.NewDense(1, 1, []float64{0}), 33 | }, 34 | mat.NewDense(1, 1, []float64{1}), // Gd 35 | 9.0, // Gamma 36 | 0.5, // AlphaR 37 | 0.3, // AlphaM 38 | ) 39 | 40 | return rose.(*roseImpl) 41 | } 42 | 43 | func TestRoseFilter(t *testing.T) { 44 | ctx := newRoseContext() 45 | filter := newRoseFilter() 46 | 47 | ctrl := mat.NewVecDense(1, nil) 48 | 49 | // init filter for comparison testing 50 | y := filter.Std.Lti.Response(ctx.X, ctrl) 51 | filter.Rose.E1 = y 52 | filter.Rose.EE1.Mul(y, y.T()) 53 | 54 | config := []struct { 55 | Iter int 56 | Input []float64 57 | Expected []float64 58 | }{ 59 | { 60 | Iter: 1, 61 | Input: []float64{ 62 | 0.04280385872149909, 63 | }, 64 | Expected: []float64{ 65 | 0.04280385872149909, 66 | }, 67 | }, 68 | { 69 | Iter: 2, 70 | Input: []float64{ 71 | -0.09725182469943415, 72 | }, 73 | Expected: []float64{ 74 | 0.04280385872149909, 75 | }, 76 | }, 77 | { 78 | Iter: 3, 79 | Input: []float64{ 80 | 0.002742478388650294, 81 | }, 82 | Expected: []float64{ 83 | 0.04280385872149909, 84 | }, 85 | }, 86 | } 87 | 88 | for _, cfg := range config { 89 | z := mat.NewVecDense(1, cfg.Input) 90 | filteredResult := filter.Apply(ctx, z, ctrl) 91 | expectedResult := mat.NewVecDense(1, cfg.Expected) 92 | if !mat.EqualApprox(expectedResult, filteredResult, 1e-4) { 93 | fmt.Println("actual:", filteredResult) 94 | fmt.Println("expected:", expectedResult) 95 | t.Error("ApplyFilter:", cfg.Iter) 96 | } 97 | } 98 | 99 | } 100 | --------------------------------------------------------------------------------