2x1=10

because numbers are people, too
Persönliches
Fotografie
Programmierung
    • libfixkalman: Fixed-Point Kalman Filter in C

      In need for a Kalman fil­ter on an embed­ded sys­tem I was look­ing for a lin­ear alge­bra library. It turned out that there are quite a bunch of libraries writ­ten in C++, most­ly tem­plate based, yet noth­ing lean and mean writ­ten in ANSI C. I end­ed up port­ing parts of EJML to C and picked only what I need­ed for the Kalman fil­ter (see the result, kalman-clib, if you are inter­est­ed). The result was a work­ing, rel­a­tive­ly fast library using floats. Or dou­bles, if the user prefers.

      How­ev­er, a big prob­lem is that many embed­ded tar­gets (say, ARM Cor­tex-M0/3) don’t sport a float­ing-point unit, so the search went on for a fixed-point lin­ear alge­bra library. Thanks to a hint on Stack­Over­flow it end­ed at lib­fix­math, which imple­ments Q16 num­bers (i.e. 16.16 on 32bit inte­ger) and is released under an MIT license.

      Luck­i­ly, some oth­er guy cre­at­ed a lin­ear alge­bra library around it, called lib­fix­ma­trix (MIT, too). I start­ed to port my Kalman library to lib­fix­ma­trix and, voilà, lib­fixkalman was born (you can find it in the lib­fixkalman GitHub repos­i­to­ry).

      Libfixkalman Usage Example

      Using lib­fixkalman should be rel­a­tive­ly sim­ple. For the fil­ter there’s a struc­ture kalman16_t and for each dif­fer­ent kind of obser­va­tion, there’s kalman16_observation_t.

      kalman16_t kf;
      kalman16_observation_t kfm;
      

      Defin­ing the fil­ter is pret­ty straight­for­ward (albeit a lit­tle nasty because of a lot of calls to float-to-fixed con­ver­sion meth­ods, like fix16_from_float()):

      #define KALMAN_NUM_STATES 3
      #define KALMAN_NUM_INPUTS 0
      #define KALMAN_NUM_MEASUREMENTS 1
      
      #define matrix_set(matrix, row, column, value) \
          matrix->data[row][column] = value
      
      static void kalman_gravity_init()
      {
          /************************************************************************/
          /* initialize the filter structures                                     */
          /************************************************************************/
          kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS);
          kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS);
      
          /************************************************************************/
          /* set initial state                                                    */
          /************************************************************************/
          mf16 *x = kalman_get_state_vector(&kf);
          matrix_set(x, 0, 0, 0); // s_i
          matrix_set(x, 1, 0, 0); // v_i
          matrix_set(x, 2, 0, F16(6)); // g_i, initial estimation
      
          /************************************************************************/
          /* set state transition                                                 */
          /************************************************************************/
          mf16 *A = kalman_get_state_transition(&kf);
         
          // set time constant and helper
          const fix16_t T = fix16_one;
          const fix16_t Tsquare = fix16_sq(T);
          const fix16_t fix16_half = F16(0.5);
      
          // transition of x to s
          matrix_set(A, 0, 0, fix16_one);   // 1
          matrix_set(A, 0, 1, T);   // T
          matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2
          
          /* ... snip ... */
      
          /************************************************************************/
          /* set covariance                                                       */
          /************************************************************************/
          mf16 *P = kalman_get_system_covariance(&kf);
      
          /* ... snip ... */
      
          /************************************************************************/
          /* set measurement transformation                                       */
          /************************************************************************/
          mf16 *H = kalman_get_observation_transformation(&kfm);
      
          /* ... snip ... */
      
          /************************************************************************/
          /* set process noise                                                    */
          /************************************************************************/
          mf16 *R = kalman_get_observation_process_noise(&kfm);
      
          /* ... snip ... */
      }
      

      After that, fil­ter­ing works by recur­sive­ly call­ing kalman_predict() and kalman_correct()

      void kalman_gravity_demo()
      {
          // initialize the filter
          kalman_gravity_init();
      
          mf16 *x = kalman_get_state_vector(&kf);
          mf16 *z = kalman_get_observation_vector(&kfm);
      
          // filter!
          uint_fast16_t i;
          for (i = 0; i < MEAS_COUNT; ++i)
          {
              // prediction.
              kalman_predict(&kf);
      
              // measure ...
              fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
              matrix_set(z, 0, 0, measurement);
      
              // update
              kalman_correct(&kf, &kfm);
          }
      
          // fetch estimated g
          const fix16_t g_estimated = x->data[2][0];
          const float value = fix16_to_float(g_estimated);
      }
      

      The hot spot, the inver­sion of the resid­ual covari­ance matrix in the obser­va­tion update step, is imple­ment­ed using a Cholesky decom­po­si­tion and an inver­sion method that is spe­cial­ized for work­ing on low­er-tri­an­gu­lar­ized pos­i­tive-def­i­nite sym­met­ric matri­ces.

      The whole exam­ple can be found in example_gravity.c.

      My afore­men­tioned float­ing-point based library kalman-clib works sim­i­lar­ly, but is a bit more dif­fi­cult to set up because of the use of shared tem­po­rary buffers. Like lib­fixkalman it avoids dynam­ic mem­o­ry allo­ca­tion, but takes anoth­er approach than lib­fix­ma­trix does.

      Januar 18th, 2014 GMT +1 von
      Markus
      2014-01-18T17:35:33+01:00 2014-04-10T03:16:16+01:00 · 10 Kommentare
      Sensor Fusion
      Signal Processing Kalman Filter Robotik

      10 Kommentare auf „libfixkalman: Fixed-Point Kalman Filter in C“

      1. Salman Sheikh sagt:
        Freitag, August 15th, 2014 02:40 am GMT +1 um 02:40 Uhr

        how do you com­pile example_gravity.c?

        Antworten
        • Markus sagt:
          Samstag, August 16th, 2014 02:09 pm GMT +1 um 14:09 Uhr

          You sim­ply com­pile it with the rest of the sources as a bina­ry. Noth­ing spe­cial here.

          Antworten
          • Tom sagt:
            Mittwoch, November 18th, 2015 11:16 pm GMT +1 um 23:16 Uhr

            That is not help­ful at all.

            Antworten
            • Markus sagt:
              Donnerstag, November 19th, 2015 02:13 am GMT +1 um 02:13 Uhr

              I’m sad you feel that way. Have a look at the devel­op branch on GitHub; that might help you more. You’ll find a Visu­al Stu­dio solu­tion and project there that includes lib­fix­math and lib­fixkalman and sta­t­i­cal­ly com­piles them with lib­fixkalman into one bina­ry that has the exam­ple code as its entry point.

              Antworten
      2. Salman Sheikh sagt:
        Freitag, August 15th, 2014 04:07 pm GMT +1 um 16:07 Uhr

        How do I com­pile this in cyg­win? I don’t have visu­al C.

        Antworten
        • Markus sagt:
          Samstag, August 16th, 2014 02:11 pm GMT +1 um 14:11 Uhr

          I did not do this, but a sim­ple make­file should do it. You’ll require lib­fix­math and lib­fix­ma­trix of course.

          Antworten
      3. Salman Sheikh sagt:
        Freitag, August 15th, 2014 10:20 pm GMT +1 um 22:20 Uhr

        Does this com­pile with gcc in Win­dows?

        Antworten
        • Markus sagt:
          Donnerstag, November 19th, 2015 02:14 am GMT +1 um 02:14 Uhr

          It’s been a while and I didn’t try, but I have no rea­son to doubt that.

          Antworten
      4. Martin sagt:
        Dienstag, März 3rd, 2015 03:10 am GMT +1 um 03:10 Uhr

        Hal­lo,

        vie­len Dank für lib­fixkalman. Ich teste sie zur Zeit auf einem ARM7 Autopi­lot. Ich habe es mir inzwis­chen selb­st recht gut zusam­men­gereimt, aber hast du vielle­icht noch Beispiel­code mit mehreren Mess­werten und/oder Con­trols?

        Antworten
        • Markus sagt:
          Dienstag, März 3rd, 2015 08:59 am GMT +1 um 08:59 Uhr

          Hi Mar­tin,

          lei­der wenig …
          Für ein Robot­er­pro­jekt habe ich kür­zlich das Fil­ter für die Odome­trie einge­set­zt, aber der Code ist aus ver­schiede­nen Grün­den nicht auf GitHub. Die rel­e­van­ten Dateien (ins­beson­dere die zur Geschwindigkeits­fu­sion) habe ich hier als Gist öffentlich gemacht, vielle­icht hil­ft Dir das etwas.

          Antworten

      Hinterlasse einen Kommentar

      Hier klicken, um das Antworten abzubrechen.

    1. « newer
    2. 1
    3. …
    4. 30
    5. 31
    6. 32
    7. 33
    8. 34
    9. 35
    10. 36
    11. …
    12. 43
    13. older »
    • Kategorien

      • .NET
        • ASP.NET
        • Core
        • DNX
      • Allgemein
      • Android
      • Data Science
      • Embedded
      • FPGA
      • Humor
      • Image Processing
      • Kalman Filter
      • Machine Learning
        • Caffe
        • Hidden Markov Models
        • ML Summarized
        • Neural Networks
        • TensorFlow
      • Mapping
      • MATLAB
      • Robotik
      • Rust
      • Signal Processing
      • Tutorial
      • Version Control
    • Neueste Beiträge

      • Summarized: The E-Dimension — Why Machine Learning Doesn’t Work Well for Some Problems?
      • Use your conda environment in Jupyter Notebooks
      • Building OpenCV for Anaconda Python 3
      • Using TensorFlow’s Supervisor with TensorBoard summary groups
      • Getting an image into and out of TensorFlow
    • Kategorien

      .NET Allgemein Android ASP.NET Caffe Core Data Science DNX Embedded FPGA Hidden Markov Models Humor Image Processing Kalman Filter Machine Learning Mapping MATLAB ML Summarized Neural Networks Robotik Rust Signal Processing TensorFlow Tutorial Version Control
    • Tags

      .NET Accelerometer Anaconda Bitmap Bug Canvas CLR docker FPGA FRDM-KL25Z FRDM-KL26Z Freescale git Gyroscope Integration Drift Intent J-Link Linear Programming Linux Magnetometer Matlab Mono Naismith OpenCV Open Intents OpenSDA Optimization Pipistrello Player/Stage PWM Python Sensor Fusion Simulink Spartan 6 svn tensorflow Tilt Compensation TRIAD ubuntu Windows Xilinx Xilinx SDK ZedBoard ZYBO Zynq
    • Letzte Kommetare

      • Lecke Mio bei Frequency-variable PWM generator in Simulink
      • Vaibhav bei Use your conda environment in Jupyter Notebooks
      • newbee bei Frequency-variable PWM generator in Simulink
      • Markus bei Using TensorFlow’s Supervisor with TensorBoard summary groups
      • Toke bei Using TensorFlow’s Supervisor with TensorBoard summary groups
    • Blog durchsuchen

    • Januar 2014
      M D M D F S S
      « Dez   Feb »
       12345
      6789101112
      13141516171819
      20212223242526
      2728293031  
    • Self

      • Find me on GitHub
      • Google+
      • Me on Stack­Ex­change
      • Ye olde blog
    • Meta

      • Anmelden
      • Beitrags-Feed (RSS)
      • Kommentare als RSS
      • WordPress.org
    (Generiert in 0,317 Sekunden)

    Zurück nach oben.