diff --git a/README.md b/README.md index 73a7dfc..7af5332 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,19 @@ # LSM9DS1-Adafruit-AHRS -A fork of jremington's AHRS library for the Adafruit LSM9DS1 \ No newline at end of file +This library is a Mahony-based AHRS and Tilt compensated compass for ESP32s and the LSM9DS1 sensor, written and tested for the Adafruit LSM9DS1 breakout board, using I2C connection on a Tiny S3 (ESP32) using PlatformIO. This library is a a fork of jremington's AHRS library for the sensor of the same name, however it utilizes [Adafruits's LSM9DS1](https://github.com/adafruit/Adafruit_LSM9DS1) library instead of the Sparkfun one and operates using C++ instead of Arduino conventions. + +![Image of the IMU](image url "https://cdn-shop.adafruit.com/970x728/4634-12.jpg") + +This library was written to support the Ringinator project and to better understand the science behind Mahony. + +# Installation + +WIP + +## How to use + +# Dependencies + +- [Adafruit LSM9DS1](https://github.com/adafruit/Adafruit_LSM9DS1) +- [Adafruit Unified Sensor Driver](https://github.com/adafruit/Adafruit_Sensor) +- Arduino Framework \ No newline at end of file diff --git a/calibration/cal_a.csv b/calibration/cal_a.csv new file mode 100644 index 0000000..3d641d2 --- /dev/null +++ b/calibration/cal_a.csv @@ -0,0 +1,300 @@ +-0.26, -0.04, 9.69 +-0.28, -0.03, 9.69 +-0.26, -0.05, 9.72 +-0.26, -0.02, 9.72 +-0.24, -0.04, 9.69 +-0.26, -0.02, 9.73 +-0.26, -0.02, 9.71 +-0.24, -0.03, 9.72 +-0.29, -0.09, 9.72 +-0.29, -0.10, 9.73 +-0.31, -0.01, 9.73 +-0.27, -0.02, 9.74 +-0.42, 0.16, 9.77 +-0.35, -0.31, 9.63 +-0.22, 0.10, 9.79 +-0.02, -0.12, 9.72 +-0.57, 0.09, 9.61 +0.15, -0.25, 10.08 +-0.03, -0.32, 9.39 +-0.21, -0.02, 9.64 +-0.47, 0.40, 9.94 +-0.28, 1.69, 9.74 +-0.02, 2.69, 9.50 +-0.12, 3.78, 8.59 +-0.20, 5.32, 8.42 +-0.56, 6.85, 7.01 +-0.60, 7.84, 5.80 +-0.94, 8.14, 5.36 +-0.93, 8.54, 4.99 +-1.31, 8.60, 4.16 +-0.78, 9.93, 3.39 +-1.52, 9.53, 2.64 +-1.43, 9.78, 1.18 +-1.79, 9.80, 0.56 +-1.23, 10.22, -0.88 +-1.16, 9.86, -2.69 +-1.25, 9.24, -3.08 +-1.21, 8.59, -4.83 +-0.79, 8.13, -6.46 +-0.78, 6.77, -7.19 +-0.70, 6.85, -8.76 +-0.75, 5.97, -8.81 +-0.78, 4.66, -9.21 +-0.95, 3.80, -9.32 +-0.53, 2.37, -9.48 +-0.46, 2.16, -9.52 +-0.35, 1.94, -9.41 +-0.44, 1.27, -9.72 +-0.04, 0.34, -10.01 +-0.26, -0.17, -9.96 +-0.16, 0.01, -10.36 +-0.05, 2.74, -11.08 +-0.52, 3.27, -9.79 +-0.02, 5.93, -10.12 +-0.17, 5.70, -7.44 +-0.58, 7.24, -6.14 +-0.62, 8.71, -4.60 +-0.75, 9.07, -2.07 +-0.44, 9.90, 0.31 +-0.74, 9.49, 1.89 +-0.42, 9.39, 3.51 +-0.01, 8.34, 3.96 +-0.56, 7.66, 4.73 +-0.66, 5.89, 7.47 +-0.57, 5.83, 8.00 +-0.96, 4.25, 8.36 +-0.13, 3.86, 9.38 +0.13, 2.73, 9.55 +-0.60, 2.05, 9.89 +-0.17, 0.39, 9.63 +-0.47, -0.08, 10.62 +-0.21, -0.67, 9.51 +0.69, -0.68, 9.89 +0.24, -1.85, 9.44 +-0.17, -4.23, 8.37 +0.26, -5.26, 8.28 +0.36, -6.30, 7.30 +0.11, -7.30, 6.02 +0.30, -8.72, 4.47 +0.49, -9.17, 3.50 +0.59, -9.21, 2.55 +0.42, -9.82, 0.84 +0.49, -9.68, -0.21 +0.38, -9.73, -1.50 +0.28, -9.35, -2.72 +-0.05, -8.85, -3.18 +0.14, -8.91, -3.57 +0.32, -8.18, -4.76 +-0.14, -7.83, -5.57 +-0.08, -7.70, -7.73 +-0.45, -6.17, -8.04 +-0.49, -5.98, -8.08 +-0.47, -4.42, -8.75 +-0.57, -3.77, -9.28 +-0.50, -3.48, -9.50 +-0.58, -2.82, -9.38 +-0.28, -2.29, -9.43 +0.00, -1.79, -10.13 +-0.46, -1.76, -9.67 +-0.29, -1.38, -9.60 +-0.16, -2.01, -9.31 +-0.03, -2.92, -10.01 +0.21, -3.87, -9.71 +0.06, -5.31, -7.49 +0.46, -7.13, -6.27 +0.37, -8.93, -4.46 +0.14, -9.70, -2.15 +0.12, -9.88, 0.55 +-0.65, -9.07, 3.45 +-0.17, -8.19, 5.05 +0.61, -6.49, 6.95 +0.90, -4.80, 7.90 +1.37, -4.04, 8.77 +1.79, -2.57, 9.24 +2.02, -1.80, 8.59 +2.04, -1.46, 8.01 +2.88, -2.05, 10.06 +3.01, -2.08, 8.44 +3.95, -0.22, 8.75 +5.01, -0.41, 8.33 +6.03, -0.48, 7.47 +6.72, 0.06, 6.69 +7.67, -0.03, 5.50 +8.18, -0.22, 4.26 +8.83, 0.47, 2.82 +9.30, 0.90, 2.54 +9.51, 0.07, -1.54 +9.21, 0.78, -2.27 +8.52, 0.75, -3.62 +6.68, 0.11, -6.21 +5.85, 0.15, -7.59 +5.37, 0.53, -7.98 +4.36, -0.08, -9.93 +4.21, 0.54, -9.13 +2.82, 0.52, -9.56 +1.41, -0.46, -9.81 +0.02, -1.02, -10.20 +-0.96, -0.46, -10.28 +-0.82, -0.04, -10.35 +-0.52, 0.27, -9.69 +0.78, -0.20, -7.73 +3.25, 0.53, -9.09 +4.75, 1.46, -9.25 +6.61, 2.06, -7.55 +7.78, 2.18, -5.25 +8.82, 2.15, -2.66 +9.08, 1.86, -0.19 +8.96, 1.35, 1.62 +8.45, 1.51, 3.25 +7.64, 0.92, 5.14 +6.05, 0.96, 7.14 +5.94, 1.16, 8.19 +4.51, 1.00, 8.86 +3.14, 1.41, 9.02 +1.60, 1.03, 9.58 +0.85, 1.59, 8.88 +-1.13, 2.47, 11.50 +-0.94, 1.09, 9.22 +-0.85, 2.03, 8.86 +-1.56, 0.51, 10.45 +-2.56, 0.88, 9.59 +-4.04, 0.82, 8.18 +-4.26, -0.11, 9.22 +-4.47, -0.45, 9.84 +-5.87, -0.33, 7.77 +-7.08, 1.36, 7.46 +-7.74, -0.65, 5.47 +-8.81, -0.93, 3.57 +-9.05, 0.32, 2.72 +-9.67, 0.30, 0.68 +-9.95, 0.54, -1.08 +-10.44, 0.80, -2.88 +-9.15, 1.11, -4.18 +-8.77, 1.01, -5.96 +-7.11, 0.14, -7.11 +-6.03, 0.01, -7.48 +-5.15, -0.00, -8.78 +-4.91, -0.11, -8.62 +-3.61, -0.22, -9.08 +-2.54, -0.33, -9.29 +-1.46, -0.23, -9.94 +-0.69, 0.22, -10.00 +-0.25, 0.39, -9.52 +0.18, 0.65, -10.02 +0.91, 0.73, -10.14 +1.45, 0.91, -9.74 +2.43, 0.65, -9.58 +2.50, 0.71, -9.58 +2.34, 0.61, -9.94 +1.28, 0.74, -9.10 +0.50, 1.50, -10.03 +-0.40, 1.80, -9.84 +-1.84, 2.75, -9.78 +-3.39, 3.43, -9.24 +-3.68, 2.58, -7.64 +-5.71, 2.90, -7.27 +-7.59, 2.75, -6.05 +-9.04, 3.05, -3.58 +-9.48, 3.05, -1.92 +-9.26, 2.95, 0.14 +-9.25, 2.67, 1.04 +-8.94, 2.86, 3.46 +-8.36, 2.22, 4.11 +-8.07, 1.97, 5.57 +-6.91, 1.50, 6.94 +-5.55, 0.66, 7.42 +-4.82, -0.41, 7.94 +-4.95, -0.84, 9.48 +-4.26, -2.58, 9.17 +-3.52, -3.51, 9.26 +-2.79, -5.36, 7.93 +-3.47, -4.60, 7.62 +-1.61, -6.82, 7.03 +-1.70, -7.26, 6.58 +-0.78, -8.44, 5.56 +-0.18, -9.02, 3.96 +0.33, -10.30, 1.01 +0.79, -9.43, 0.70 +0.62, -9.69, -1.25 +0.60, -9.24, -2.43 +0.41, -9.58, -3.19 +-0.04, -8.88, -4.51 +0.02, -7.97, -5.91 +0.24, -7.36, -6.66 +0.36, -6.67, -6.80 +0.19, -6.61, -7.57 +0.27, -5.62, -8.25 +0.07, -5.17, -8.38 +0.28, -4.46, -8.85 +0.51, -3.32, -9.27 +0.59, -3.55, -9.53 +0.73, -2.04, -9.84 +0.56, -1.60, -9.28 +0.51, -1.66, -9.62 +0.43, -1.66, -9.57 +0.54, -1.70, -10.35 +0.36, -1.24, -11.19 +-0.22, -3.00, -7.53 +-0.59, -1.83, -9.41 +-1.48, -1.49, -9.40 +-1.40, -4.70, -9.19 +-1.25, -2.54, -9.56 +-1.53, -2.86, -7.61 +-1.50, -0.69, -7.80 +-1.40, -0.68, -10.36 +-1.43, -0.11, -9.55 +-1.60, -0.41, -10.25 +-0.86, 2.39, -9.88 +-1.77, 2.87, -9.71 +-0.97, 4.24, -8.86 +-1.20, 4.69, -7.95 +-0.41, 6.46, -8.95 +-1.49, 4.67, -6.81 +-0.96, 6.58, -7.48 +-0.36, 6.82, -6.82 +-0.21, 7.27, -7.48 +0.19, 8.40, -6.09 +0.12, 8.23, -5.74 +0.17, 8.86, -4.09 +0.20, 9.59, -3.14 +0.33, 9.40, -1.25 +0.38, 9.49, -0.59 +0.64, 9.95, 0.10 +0.73, 11.32, 1.94 +0.48, 10.02, 1.49 +0.44, 9.73, 2.77 +0.70, 9.01, 3.51 +0.74, 8.32, 3.97 +0.75, 9.13, 3.91 +0.90, 7.97, 5.55 +0.53, 7.44, 5.66 +0.29, 7.91, 5.90 +0.01, 7.78, 6.24 +0.25, 7.06, 6.87 +0.21, 7.09, 6.94 +0.34, 6.95, 7.07 +0.21, 7.38, 6.50 +-0.13, 9.13, 5.19 +-0.35, 9.05, 4.98 +-0.50, 9.40, 2.82 +-0.41, 10.45, 0.91 +-0.84, 10.08, -0.85 +-0.71, 9.69, -3.50 +-0.70, 8.58, -4.99 +-0.87, 6.44, -6.92 +-0.86, 5.25, -8.22 +-0.80, 4.87, -8.79 +-0.66, 3.95, -9.81 +-0.98, 1.46, -10.02 +-1.25, -0.37, -9.94 +-1.44, -2.36, -9.89 +-2.24, -3.55, -7.90 +-2.45, -4.00, -8.69 +-2.45, -5.67, -8.10 +-2.48, -6.08, -7.39 +-2.93, -7.83, -6.83 +-3.49, -6.49, -4.17 +-3.22, -9.34, -5.12 +-3.46, -8.18, -3.50 +-3.21, -9.44, -2.96 \ No newline at end of file diff --git a/calibration/cal_m.csv b/calibration/cal_m.csv new file mode 100644 index 0000000..03d0cbd --- /dev/null +++ b/calibration/cal_m.csv @@ -0,0 +1,300 @@ + 18.43, -25.92, -59.04 + 18.23, -25.58, -58.77 + 18.68, -26.04, -58.00 + 18.61, -25.49, -59.10 + 18.71, -26.45, -59.29 + 18.87, -25.47, -60.05 + 17.89, -27.06, -59.78 + 18.25, -26.16, -60.11 + 19.70, -26.08, -59.22 + 18.15, -25.64, -59.56 + 18.79, -25.75, -59.03 + 19.01, -25.78, -58.81 + 18.86, -26.12, -59.54 + 19.08, -25.62, -59.05 + 22.18, -29.33, -56.13 + 20.84, -30.14, -56.17 + 15.73, -34.81, -54.73 + 11.98, -44.69, -48.41 + 10.18, -53.11, -39.89 + 9.77, -55.97, -34.73 + 8.21, -61.57, -23.14 + 8.87, -64.79, -13.89 + 8.52, -64.24, -2.84 + 8.74, -64.50, 8.31 + 8.79, -62.62, 16.93 + 9.27, -60.26, 22.57 + 7.89, -57.42, 31.15 + 7.98, -52.87, 37.79 + 8.79, -48.72, 42.96 + 10.68, -41.53, 49.27 + 11.44, -37.14, 51.04 + 11.47, -35.29, 53.84 + 12.24, -34.82, 53.60 + 12.80, -39.39, 50.60 + 10.69, -52.86, 36.69 + 11.12, -60.86, 20.18 + 10.21, -63.75, 6.41 + 10.16, -63.88, -7.15 + 9.65, -60.45, -21.00 + 11.22, -56.62, -31.77 + 12.14, -52.11, -40.45 + 12.72, -48.31, -43.48 + 13.25, -42.58, -48.59 + 16.53, -36.23, -53.70 + 18.49, -29.71, -56.85 + 20.39, -16.92, -60.55 + 20.33, -7.62, -62.65 + 21.87, 1.13, -61.67 + 22.76, 12.04, -60.21 + 22.92, 20.30, -57.56 + 26.39, 27.44, -53.48 + 27.45, 30.66, -51.45 + 27.47, 34.67, -49.15 + 29.13, 40.57, -43.63 + 29.03, 47.72, -37.37 + 28.43, 51.20, -30.78 + 27.81, 53.83, -24.84 + 29.75, 53.92, -20.76 + 30.95, 56.24, -16.64 + 31.20, 56.52, -13.76 + 31.32, 56.91, -9.23 + 31.28, 56.94, -7.84 + 31.74, 57.21, -8.66 + 30.90, 56.74, -14.54 + 30.60, 51.01, -27.10 + 29.96, 44.09, -39.75 + 28.47, 33.60, -48.43 + 26.33, 23.71, -55.24 + 24.24, 12.34, -61.32 + 20.84, -1.75, -62.00 + 19.86, -12.88, -61.51 + 20.62, -17.44, -59.59 + 24.70, -20.33, -57.78 + 29.11, -23.75, -55.22 + 31.59, -27.15, -50.71 + 33.20, -34.27, -46.39 + 34.10, -37.28, -41.66 + 36.58, -38.54, -38.51 + 37.01, -39.53, -35.48 + 43.22, -35.80, -32.16 + 49.81, -32.54, -25.67 + 53.61, -32.45, -15.64 + 56.98, -30.63, -7.00 + 58.14, -29.39, -0.49 + 56.56, -31.12, 10.29 + 53.94, -31.91, 20.20 + 50.93, -31.15, 29.92 + 46.90, -29.65, 37.02 + 39.41, -28.52, 44.96 + 32.30, -28.23, 51.04 + 26.07, -27.04, 55.36 + 18.71, -25.48, 59.10 + 12.08, -24.35, 62.28 + 6.31, -22.11, 63.20 + 1.01, -21.46, 63.01 + -4.26, -21.19, 67.34 + -11.29, -21.28, 62.94 + -18.39, -20.49, 61.48 + -23.40, -20.38, 59.60 + -27.80, -19.54, 57.34 + -31.03, -19.54, 56.43 + -30.87, -20.40, 55.14 + -29.73, -20.23, 56.48 + -21.98, -22.94, 59.46 + -4.19, -25.97, 60.86 + 13.55, -31.93, 55.61 + 29.17, -38.67, 45.65 + 39.28, -37.08, 38.56 + 49.30, -32.60, 28.14 + 55.85, -29.35, 17.48 + 58.80, -26.90, 6.95 + 58.34, -26.93, -2.68 + 55.17, -28.82, -16.77 + 49.22, -29.27, -27.27 + 44.95, -26.68, -34.25 + 37.99, -21.76, -44.31 + 32.40, -17.91, -49.13 + 28.15, -16.59, -50.31 + 22.34, -17.75, -53.31 + 15.02, -20.44, -53.69 + 9.53, -22.68, -55.37 + 5.51, -24.52, -55.07 + 1.61, -26.50, -55.18 + 0.18, -29.45, -54.23 + -4.48, -31.61, -53.87 + -8.28, -33.36, -51.97 + -11.56, -34.06, -51.05 + -14.88, -33.24, -51.98 + -17.12, -30.39, -52.76 + -22.57, -29.00, -51.64 + -26.82, -29.63, -50.02 + -30.79, -29.83, -46.93 + -39.25, -30.75, -42.20 + -48.77, -30.22, -30.08 + -56.31, -27.39, -18.90 + -60.16, -26.77, -8.80 + -61.26, -26.38, -1.26 + -60.48, -25.11, 7.49 + -58.20, -23.23, 17.54 + -54.16, -21.92, 27.39 + -51.42, -21.58, 34.51 + -45.16, -22.13, 41.21 + -40.46, -23.70, 43.83 + -33.56, -23.20, 48.68 + -25.29, -25.88, 51.81 + -17.76, -26.83, 54.45 + -9.67, -28.70, 55.54 + -5.88, -27.75, 55.79 + 2.49, -28.06, 56.64 + 9.51, -29.57, 55.37 + 12.77, -30.73, 54.43 + 14.90, -32.04, 54.01 + 15.50, -32.20, 53.21 + 15.15, -33.12, 53.06 + 11.37, -32.52, 53.22 + 2.45, -32.29, 55.04 + -9.59, -32.04, 53.74 + -24.73, -34.07, 48.09 + -34.65, -35.10, 40.13 + -42.66, -37.60, 32.06 + -46.81, -40.11, 22.79 + -47.59, -44.26, 9.33 + -47.68, -46.53, -5.38 + -46.55, -45.73, -15.57 + -45.40, -43.85, -24.23 + -38.89, -40.66, -37.83 + -30.44, -35.39, -46.71 + -21.10, -31.82, -53.59 + -8.80, -25.62, -59.93 + 1.42, -16.67, -62.46 + 11.78, -7.85, -62.80 + 20.28, -0.85, -61.70 + 23.01, 5.39, -60.15 + 26.61, 12.16, -57.33 + 29.80, 17.70, -54.57 + 31.07, 22.80, -52.21 + 32.00, 27.84, -48.59 + 33.36, 31.28, -45.38 + 34.56, 36.29, -41.66 + 36.63, 41.91, -34.25 + 39.45, 46.46, -23.31 + 40.45, 49.84, -13.23 + 41.31, 50.98, -6.22 + 42.17, 50.35, 3.01 + 42.60, 49.80, 8.53 + 43.37, 47.56, 15.27 + 42.85, 46.74, 16.73 + 43.98, 46.45, 18.18 + 44.15, 45.29, 19.44 + 42.32, 49.04, 11.22 + 41.23, 49.74, 7.06 + 40.44, 51.63, 0.29 + 38.26, 51.23, -13.15 + 36.01, 46.02, -28.87 + 32.21, 37.02, -43.81 + 28.58, 26.62, -51.84 + 25.84, 16.24, -57.14 + 24.69, 8.76, -58.78 + 25.88, 6.71, -59.24 + 22.46, -2.47, -61.35 + 22.17, -8.73, -59.25 + 18.64, -16.79, -59.83 + 18.18, -24.21, -58.61 + 18.30, -32.10, -53.20 + 18.56, -37.82, -49.35 + 16.45, -44.56, -45.30 + 14.81, -50.00, -40.02 + 12.37, -55.39, -33.29 + 9.59, -58.47, -26.33 + 8.43, -61.62, -18.87 + 8.67, -63.85, -10.27 + 10.86, -64.12, -4.49 + 13.22, -64.04, -4.73 + 12.46, -64.69, 3.97 + 10.96, -63.49, 11.31 + 9.78, -62.02, 18.93 + 9.49, -55.58, 32.56 + 7.97, -49.52, 41.04 + 3.20, -36.64, 52.97 + 2.41, -28.75, 56.64 + 0.26, -21.28, 61.60 + -1.24, -14.72, 62.75 + -3.74, -11.30, 63.42 + -3.79, -1.52, 65.00 + -3.91, 6.53, 64.80 + -4.16, 15.93, 62.91 + -4.79, 20.65, 61.87 + -5.72, 29.39, 58.58 + -4.70, 36.20, 54.97 + -2.65, 43.91, 47.33 + -1.77, 46.58, 45.24 + -0.80, 52.50, 39.04 + -0.46, 54.50, 35.92 + 0.69, 58.97, 26.02 + 2.77, 60.01, 22.79 + 3.02, 60.11, 23.76 + 3.53, 60.63, 24.69 + 5.00, 58.58, 27.06 + 5.35, 54.32, 34.63 + 5.68, 48.51, 43.55 + 5.96, 42.14, 48.88 + 3.15, 32.60, 56.63 + -0.61, 21.72, 61.57 + -0.75, 5.16, 64.45 + -4.08, -6.73, 64.70 + -4.56, -23.93, 58.91 + -8.01, -36.79, 51.53 + -11.88, -46.97, 41.80 + -12.92, -57.40, 26.20 + -13.86, -61.99, 7.76 + -12.18, -62.47, -13.21 + -7.78, -58.05, -28.67 + -5.00, -52.60, -38.45 + 0.84, -44.11, -47.42 + 5.87, -37.42, -52.53 + 9.40, -32.32, -54.84 + 15.28, -27.19, -57.23 + 18.40, -20.73, -58.57 + 20.71, -20.50, -59.00 + 25.47, -19.00, -56.69 + 28.55, -17.69, -56.26 + 31.79, -14.81, -54.77 + 35.26, -12.12, -52.74 + 36.78, -10.14, -52.86 + 36.78, -8.97, -54.11 + 36.98, -6.14, -53.27 + 35.09, -2.45, -55.77 + 34.01, 3.92, -56.80 + 33.51, 14.53, -52.54 + 33.73, 26.29, -48.84 + 34.40, 29.42, -47.90 + 34.04, 38.45, -41.36 + 34.72, 45.27, -30.70 + 34.84, 48.71, -22.51 + 35.36, 52.26, -13.69 + 36.92, 53.82, -4.79 + 36.12, 53.65, -0.40 + 36.78, 52.98, 6.80 + 36.70, 51.96, 12.52 + 35.93, 51.76, 11.92 + 31.82, 55.19, 6.99 + 31.58, 55.62, 1.95 + 30.37, 56.55, -4.24 + 30.30, 56.85, -7.64 + 29.96, 57.18, -7.27 + 28.04, 56.82, -10.58 + 25.12, 57.74, -13.80 + 22.15, 58.94, -16.24 + 20.89, 58.57, -18.81 + 18.20, 58.60, -21.46 + 16.52, 58.41, -23.73 + 14.23, 57.52, -26.62 + 10.98, 57.40, -30.63 + 7.42, 56.45, -31.47 + 3.60, 57.55, -33.12 + 0.78, 57.21, -33.80 + -1.35, 56.66, -33.37 + -4.40, 57.30, -31.49 + -10.02, 57.19, -31.83 + -12.57, 57.08, -32.29 diff --git a/calibration/calibration_data_collection.cpp b/calibration/calibration_data_collection.cpp new file mode 100644 index 0000000..fc29784 --- /dev/null +++ b/calibration/calibration_data_collection.cpp @@ -0,0 +1,152 @@ +/* +This code is a modified version of the standard code from +the Adafruit LSM9DS1 library. + +Example I2C hardware setup with the TinyS3: +LSM9DS1 -------- ESP32 +3V ------------- 3.3V +G -------------- GND or - +SCL ------------ 9 +SDA ------------ 8 +*/ + +#include +#include +#include +#include +#include // not used in this demo but required! + +// i2c +Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); + +#define LSM9DS1_SCK A5 +#define LSM9DS1_MISO 12 +#define LSM9DS1_MOSI A4 +#define LSM9DS1_XGCS 6 +#define LSM9DS1_MCS 5 + + +// You can also use software SPI +//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_SCK, LSM9DS1_MISO, LSM9DS1_MOSI, LSM9DS1_XGCS, LSM9DS1_MCS); +// Or hardware SPI! In this case, only CS pins are passed in +//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_XGCS, LSM9DS1_MCS); + + + + + +#define DELAY 100 // 100 ms between prints +int printCount = 0; // Counts the number of times we performed measurements +bool acc = true; // Switches between the accelerometer and magnetometer + +void setupSensor() +{ + // IMPORTANT: Accuracy is inversely proportional to the sensor range! + // Ringinator works with + // 1.) Set the accelerometer range + lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ); + + // 2.) Set the magnetometer sensitivity + lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS); + + // 3.) Setup the gyroscope + lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS); +} + + +void setup() +{ + Serial.begin(115200); + + while (!Serial) { + delay(1); // will pause Zero, Leonardo, etc until serial console opens + } + + Serial.println("LSM9DS1 data read demo"); + + // Try to initialise and warn if we couldn't detect the chip + if (!lsm.begin()) + { + Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!"); + while (1); + } + Serial.println("Found LSM9DS1 9DOF"); + + // helper to just set the default scaling we want, see above! + setupSensor(); + + delay(2000); // 2 Second delay because the TinyS3 has to be reset and the button press shakes the device + Serial.println("Collecting gyro data, hold still"); + // get gyro offset + float gxa = 0, gya = 0, gza = 0; //change to double if number is big + for (int i = 0; i < 300; i++) { + lsm.read(); /* ask it to read in the data */ + sensors_event_t a, m, g, temp; + lsm.getEvent(&a, &m, &g, &temp); + gxa += g.gyro.x; + gya += g.gyro.y; + gza += g.gyro.z; + } + Serial.println(F("gyro offsets")); + Serial.print(gxa / 300); + Serial.print(", "); + Serial.print(gya / 300); + Serial.print(", "); + Serial.println(gza / 300); + Serial.println(); + + Serial.println(F("rotate slowly and carefully in 3D")); + delay(3000); + Serial.println("starting"); + + + +} + +void loop() +{ + lsm.read(); /* ask it to read in the data */ + + /* Get a new sensor event */ + sensors_event_t a, m, g, temp; + + lsm.getEvent(&a, &m, &g, &temp); + + if (printCount < 300 && acc) { + Serial.print(a.acceleration.x); + Serial.print(", "); + Serial.print(a.acceleration.y); + Serial.print(", "); + Serial.println(a.acceleration.z); + printCount++; + } else if (printCount < 300 && !acc) { + Serial.print(m.magnetic.x); + Serial.print(", "); + Serial.print(m.magnetic.y); + Serial.print(", "); + Serial.println(m.magnetic.z); + printCount++; + } else { + Serial.println("Done."); + delay(4000); + printCount = 0; + if (acc) { + Serial.println(F("rotate slowly and carefully in 3D")); + delay(3000); + Serial.println("starting"); + acc = false; + } else { + while(1); // HALT + } + + } + delay(DELAY); +} \ No newline at end of file diff --git a/calibration/mag_calibration.py b/calibration/mag_calibration.py new file mode 100644 index 0000000..696ac69 --- /dev/null +++ b/calibration/mag_calibration.py @@ -0,0 +1,164 @@ +import numpy as np +from scipy import linalg +from matplotlib import pyplot as plt + + #https://github.com/nliaudat/magnetometer_calibration/blob/main/calibrate.py + #corrected code by S. James Remington + #required data input file: x,y,z values in .csv (text, comma separated value) format. + +class Magnetometer(object): + + ''' + references : + - https://teslabs.com/articles/magnetometer-calibration/ + - https://github.com/nliaudat/magnetometer_calibration/blob/main/calibrate.py + + ''' + MField = 1000 #arbitrary norm of magnetic field vectors + + def __init__(self, F=MField): + + + # initialize values + self.F = F + self.b = np.zeros([3, 1]) + self.A_1 = np.eye(3) + + def run(self): + + data = np.loadtxt("cal_m.csv",delimiter=',') + print("shape of data array:",data.shape) + #print("datatype of data:",data.dtype) + print("First 5 rows raw:\n", data[:5]) + + # ellipsoid fit + s = np.array(data).T + M, n, d = self.__ellipsoid_fit(s) + + # calibration parameters + M_1 = linalg.inv(M) + self.b = -np.dot(M_1, n) + self.A_1 = np.real(self.F / np.sqrt(np.dot(n.T, np.dot(M_1, n)) - d) * linalg.sqrtm(M)) + + #print("M:\n", M, "\nn:\n", n, "\nd:\n", d) + #print("M_1:\n",M_1, "\nb:\n", self.b, "\nA_1:\n", self.A_1) + + print("\nData normalized to ",self.F) + print("Soft iron transformation matrix:\n",self.A_1) + print("Hard iron bias:\n", self.b) + + plt.rcParams["figure.autolayout"] = True + #fig = plt.figure() + #ax = fig.add_subplot(111, projection='3d') + #ax.scatter(data[:,0], data[:,1], data[:,2], marker='o', color='r') + #plt.show() + + result = [] + for row in data: + + # subtract the hard iron offset + xm_off = row[0]-self.b[0] + ym_off = row[1]-self.b[1] + zm_off = row[2]-self.b[2] + + #multiply by the inverse soft iron offset + xm_cal = xm_off * self.A_1[0,0] + ym_off * self.A_1[0,1] + zm_off * self.A_1[0,2] + ym_cal = xm_off * self.A_1[1,0] + ym_off * self.A_1[1,1] + zm_off * self.A_1[1,2] + zm_cal = xm_off * self.A_1[2,0] + ym_off * self.A_1[2,1] + zm_off * self.A_1[2,2] + + result = np.append(result, np.array([xm_cal, ym_cal, zm_cal]) )#, axis=0 ) + + result = result.reshape(-1, 3) + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ax.scatter(result[:,0], result[:,1], result[:,2], marker='o', color='g') + plt.show() + + print("First 5 rows calibrated:\n", result[:5]) + + #save corrected data to file "out.txt" + np.savetxt('out.txt', result, fmt='%f', delimiter=' ,') + + print("*************************" ) + print("code to paste : " ) + print("*************************" ) + self.b = np.round(self.b,2) + print("float B[3] = {", self.b[0],",", self.b[1],",", self.b[2],"};") + print("\n") + + self.A_1 = np.round(self.A_1,5) + print("float A_inv[3][3] = {") + print("{", self.A_1[0,0],",", self.A_1[0,1],",", self.A_1[0,2], "},") + print("{", self.A_1[1,0],",", self.A_1[1,1],",", self.A_1[2,1], "},") + print("{", self.A_1[2,0],",", self.A_1[2,1],",", self.A_1[2,2], "}};") + print("\n") + + + + def __ellipsoid_fit(self, s): + ''' Estimate ellipsoid parameters from a set of points. + + Parameters + ---------- + s : array_like + The samples (M,N) where M=3 (x,y,z) and N=number of samples. + + Returns + ------- + M, n, d : array_like, array_like, float + The ellipsoid parameters M, n, d. + + References + ---------- + .. [1] Qingde Li; Griffiths, J.G., "Least squares ellipsoid specific + fitting," in Geometric Modeling and Processing, 2004. + Proceedings, vol., no., pp.335-340, 2004 + ''' + + # D (samples) + D = np.array([s[0]**2., s[1]**2., s[2]**2., + 2.*s[1]*s[2], 2.*s[0]*s[2], 2.*s[0]*s[1], + 2.*s[0], 2.*s[1], 2.*s[2], np.ones_like(s[0])]) + + # S, S_11, S_12, S_21, S_22 (eq. 11) + S = np.dot(D, D.T) + S_11 = S[:6,:6] + S_12 = S[:6,6:] + S_21 = S[6:,:6] + S_22 = S[6:,6:] + + # C (Eq. 8, k=4) + C = np.array([[-1, 1, 1, 0, 0, 0], + [ 1, -1, 1, 0, 0, 0], + [ 1, 1, -1, 0, 0, 0], + [ 0, 0, 0, -4, 0, 0], + [ 0, 0, 0, 0, -4, 0], + [ 0, 0, 0, 0, 0, -4]]) + + # v_1 (eq. 15, solution) + E = np.dot(linalg.inv(C), + S_11 - np.dot(S_12, np.dot(linalg.inv(S_22), S_21))) + + E_w, E_v = np.linalg.eig(E) + + v_1 = E_v[:, np.argmax(E_w)] + if v_1[0] < 0: v_1 = -v_1 + + # v_2 (eq. 13, solution) + v_2 = np.dot(np.dot(-np.linalg.inv(S_22), S_21), v_1) + + # quadratic-form parameters, parameters h and f swapped as per correction by Roger R on Teslabs page + M = np.array([[v_1[0], v_1[5], v_1[4]], + [v_1[5], v_1[1], v_1[3]], + [v_1[4], v_1[3], v_1[2]]]) + n = np.array([[v_2[0]], + [v_2[1]], + [v_2[2]]]) + d = v_2[3] + + return M, n, d + + + +if __name__=='__main__': + Magnetometer().run() \ No newline at end of file diff --git a/calibration/magnetoCalibrator b/calibration/magnetoCalibrator new file mode 100755 index 0000000..ec159a0 Binary files /dev/null and b/calibration/magnetoCalibrator differ diff --git a/calibration/magnetoCalibrator.c b/calibration/magnetoCalibrator.c new file mode 100644 index 0000000..666a715 --- /dev/null +++ b/calibration/magnetoCalibrator.c @@ -0,0 +1,2392 @@ +// This version of Magneto is authored by S. James Remington +// magneto 1.4 magnetometer/accelerometer calibration code +// from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html +// tested and works with Code::Blocks 10.05 through 20.03 + +// Command line version slightly modified from original, sjames_remington@gmail.com +// Now includes option to reject outliers + +// comma separated ASCII input data file expected, three columns x, y, z + +#include +#include +#include +#include +#include + +// Forward declarations of mymathlib.com routines +void Multiply_Self_Transpose(double*, double*, int, int); +void Get_Submatrix(double*, int, int, double*, int, int, int); +int Choleski_LU_Decomposition(double*, int); +int Choleski_LU_Inverse(double*, int); +void Multiply_Matrices(double*, double*, int, int, double*, int); +void Identity_Matrix(double*, int); + +int Hessenberg_Form_Elementary(double*, double*, int); +static void Hessenberg_Elementary_Transform(double*, double*, int[], int); + +void Copy_Vector(double*, double*, int); + +int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int); +static void One_Real_Eigenvalue(double[], double[], double[], int, double); +static void Two_Eigenvalues(double*, double*, double[], double[], int, int, double); +static void Update_Row(double*, double, double, int, int); +static void Update_Column(double*, double, double, int, int); +static void Update_Transformation(double*, double, double, int, int); +static void Double_QR_Iteration(double*, double*, int, int, int, double*, int); +static void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int); +static int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double); +static void Double_QR_Step(double*, int, int, int, double, double, double*, int); +static void BackSubstitution(double*, double[], double[], int); +static void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int); +static void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int); +static void Calculate_Eigenvectors(double*, double*, double[], double[], int); +static void Complex_Division(double, double, double, double, double*, double*); + +void Transpose_Square_Matrix(double*, int); + +// +// *** CODE STARTS HERE *** +// +int main() +{ + char buf[120]; + double *D, *S, *C, *S11, *S12, *S12t, *S22, *S22_1, *S22a, *S22b, *SS, *E, *d, *U, *SSS; + double *eigen_real, *eigen_imag, *v1, *v2, *v, *Q, *Q_1, *B, *QB, J, hmb, *SSSS; + int *p; + int i, j, index; + double maxval, norm, btqb, *eigen_real3, *eigen_imag3, *Dz, *vdz, *SQ, *A_1, hm, norm1, norm2, norm3; + double x, y, z, x2, nxsrej, xs, xave; + double *raw; //raw obs + + int nlines = 0, ngood=0; //count measurements + + char filename[64]; + FILE *fp, *fp2; + +// this version reads comma separated data + + printf("\r\nMagneto 1.4 2/10/2021\r\nInput .csv file? "); + scanf("%s",&filename); + + fp = fopen(filename, "r"); + if (fp == NULL) {printf("file not found"); return 0;} +// get the number of lines in the file + while(fgets(buf, 100, fp) != NULL) + nlines++; + rewind(fp); + + // + // calculate mean (norm) and standard deviation for possible outlier rejection + // + xs=0; + xave=0; +for( i = 0; i < nlines; i++) + { + fgets(buf, 100, fp); + index=sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); + printf("%3d %6.0f %6.0f %6.0f\n",i,x,y,z); + x2 = x*x + y*y + z*z; + xs += x2; + xave += sqrt(x2); + } + xave = xave/nlines; //mean vector length + xs = sqrt(xs/nlines -(xave*xave)); //std. dev. + rewind(fp); + +// summarize statistics, give user opportunity to reject outlying measurements + + printf("\r\nAverage magnitude (default Hm) and sigma of %d vectors = %6.1lf, %6.1lf\r\n",nlines,xave,xs); + printf("\r\nReject outliers? (0 or d, reject if > d*sigma from mean) "); + scanf("%lf",&nxsrej); + printf("Rejection level selected: %4.1f\r\n ",nxsrej); + + // scan file again + + ngood = nlines; //count good + if (nxsrej > 0) { + printf("\r\nRejecting measurements if abs(vector_length-average)/(std. dev.) > %5.1f\r\n",nxsrej); + + // outlier rejection, count remaining lines +for( i = 0; i < nlines; i++) + { + fgets(buf, 100, fp); + index=sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); + x2 = sqrt(x*x + y*y + z*z); //vector length + x2 =fabs(x2 - xave)/xs; //standard deviations from mean + if (x2 > nxsrej) { + ngood--; + printf("reject #%d, %8.1f %8.1f %8.1f\n",i, x,y,z); + } + } + printf("\r\nNumber of measurements to be rejected: %d, accepted: %d\r\n",nlines-ngood,ngood); + rewind(fp); + } + + // third time through! + // allocate array space for accepted measurements + D = (double*)malloc(10 * ngood * sizeof(double)); + raw = (double*)malloc(3 * ngood * sizeof(double)); + + j = 0; //array index for good measurements +// printf("\r\nAccepted measurements (file index, internal index, ...)\r\n"); + for( i = 0; i < nlines; i++) + { + fgets(buf, 100, fp); + index=sscanf(buf, "%lf,%lf,%lf", &x, &y, &z); //comma separated values + x2 = sqrt(x*x + y*y + z*z); //vector length + x2 = fabs(x2 - xave)/xs; //standard deviation from mean + if ((nxsrej == 0) || (x2 <= nxsrej)) { +// accepted measurement +// printf("%d, %d: %6.1f %6.1f %6.1f\r\n",i,j,x,y,z); + + raw[3*j] = x; + raw[3*j+1] = y; + raw[3*j+2] = z; + D[j] = x * x; + D[ngood+j] = y * y; + D[ngood*2+j] = z * z; + D[ngood*3+j] = 2.0 * y * z; + D[ngood*4+j] = 2.0 * x * z; + D[ngood*5+j] = 2.0 * x * y; + D[ngood*6+j] = 2.0 * x; + D[ngood*7+j] = 2.0 * y; + D[ngood*8+j] = 2.0 * z; + D[ngood*9+j] = 1.0; + j++; //count good measurements + } + } + fclose(fp); + + printf("\r\n%3d measurements processed, expected %d\r\n",j,ngood); + if (j != ngood) {printf("Internal indexing error!\r\n"); return 0; } + + nlines = ngood; //number to process + + printf("\r\nExpected norm of local field vector Hm? (Enter 0 for default %8.1f) ",xave); + scanf("%lf",&hm); + + if(hm == 0.0) hm=xave; + printf("\r\nSet Hm = %8.1f\r\n",hm); + + // allocate memory for matrix S + S = (double*)malloc(10 * 10 * sizeof(double)); + Multiply_Self_Transpose(S, D, 10, nlines); + + // Create pre-inverted constraint matrix C + C = (double*)malloc(6 * 6 * sizeof(double)); + C[0] = 0.0; C[1] = 0.5; C[2] = 0.5; C[3] = 0.0; C[4] = 0.0; C[5] = 0.0; + C[6] = 0.5; C[7] = 0.0; C[8] = 0.5; C[9] = 0.0; C[10] = 0.0; C[11] = 0.0; + C[12] = 0.5; C[13] = 0.5; C[14] = 0.0; C[15] = 0.0; C[16] = 0.0; C[17] = 0.0; + C[18] = 0.0; C[19] = 0.0; C[20] = 0.0; C[21] = -0.25; C[22] = 0.0; C[23] = 0.0; + C[24] = 0.0; C[25] = 0.0; C[26] = 0.0; C[27] = 0.0; C[28] = -0.25; C[29] = 0.0; + C[30] = 0.0; C[31] = 0.0; C[32] = 0.0; C[33] = 0.0; C[34] = 0.0; C[35] = -0.25; + + S11 = (double*)malloc(6 * 6 * sizeof(double)); + Get_Submatrix(S11, 6, 6, S, 10, 0, 0); + S12 = (double*)malloc(6 * 4 * sizeof(double)); + Get_Submatrix(S12, 6, 4, S, 10, 0, 6); + S12t = (double*)malloc(4 * 6 * sizeof(double)); + Get_Submatrix(S12t, 4, 6, S, 10, 6, 0); + S22 = (double*)malloc(4 * 4 * sizeof(double)); + Get_Submatrix(S22, 4, 4, S, 10, 6, 6); + + S22_1 = (double*)malloc(4 * 4 * sizeof(double)); + for(i = 0; i < 16; i++) + S22_1[i] = S22[i]; + Choleski_LU_Decomposition(S22_1, 4); + Choleski_LU_Inverse(S22_1, 4); + + // Calculate S22a = S22_1 * S12t 4*6 = 4x4 * 4x6 C = AB + S22a = (double*)malloc(4 * 6 * sizeof(double)); + Multiply_Matrices(S22a, S22_1, 4, 4, S12t, 6); + + // Then calculate S22b = S12 * S22a ( 6x6 = 6x4 * 4x6) + S22b = (double*)malloc(6 * 6 * sizeof(double)); + Multiply_Matrices(S22b, S12, 6, 4, S22a, 6); + + // Calculate SS = S11 - S22b + SS = (double*)malloc(6 * 6 * sizeof(double)); + for(i = 0; i < 36; i++) + SS[i] = S11[i] - S22b[i]; + E = (double*)malloc(6 * 6 * sizeof(double)); + Multiply_Matrices(E, C, 6, 6, SS, 6); + + SSS = (double*)malloc(6 * 6 * sizeof(double)); + Hessenberg_Form_Elementary(E, SSS, 6); + + eigen_real = (double*)malloc(6 * sizeof(double)); + eigen_imag = (double*)malloc(6 * sizeof(double)); + + QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100); + + index = 0; + maxval = eigen_real[0]; + for(i = 1; i < 6; i++) + { + if(eigen_real[i] > maxval) + { + maxval = eigen_real[i]; + index = i; + } + } + + v1 = (double*)malloc(6 * sizeof(double)); + + v1[0] = SSS[index]; + v1[1] = SSS[index+6]; + v1[2] = SSS[index+12]; + v1[3] = SSS[index+18]; + v1[4] = SSS[index+24]; + v1[5] = SSS[index+30]; + + // normalize v1 + norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]); + v1[0] /= norm; + v1[1] /= norm; + v1[2] /= norm; + v1[3] /= norm; + v1[4] /= norm; + v1[5] /= norm; + + if(v1[0] < 0.0) + { + v1[0] = -v1[0]; + v1[1] = -v1[1]; + v1[2] = -v1[2]; + v1[3] = -v1[3]; + v1[4] = -v1[4]; + v1[5] = -v1[5]; + } + + // Calculate v2 = S22a * v1 ( 4x1 = 4x6 * 6x1) + v2 = (double*)malloc(4 * sizeof(double)); + Multiply_Matrices(v2, S22a, 4, 6, v1, 1); + + v = (double*)malloc(10 * sizeof(double)); + + v[0] = v1[0]; + v[1] = v1[1]; + v[2] = v1[2]; + v[3] = v1[3]; + v[4] = v1[4]; + v[5] = v1[5]; + v[6] = -v2[0]; + v[7] = -v2[1]; + v[8] = -v2[2]; + v[9] = -v2[3]; + + Q = (double*)malloc(3 * 3 * sizeof(double)); + + Q[0] = v[0]; + Q[1] = v[5]; + Q[2] = v[4]; + Q[3] = v[5]; + Q[4] = v[1]; + Q[5] = v[3]; + Q[6] = v[4]; + Q[7] = v[3]; + Q[8] = v[2]; + + U = (double*)malloc(3 * sizeof(double)); + + U[0] = v[6]; + U[1] = v[7]; + U[2] = v[8]; + Q_1 = (double*)malloc(3 * 3 * sizeof(double)); + for(i = 0; i < 9; i++) + Q_1[i] = Q[i]; + Choleski_LU_Decomposition(Q_1, 3); + Choleski_LU_Inverse(Q_1, 3); + + // Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1) + B = (double*)malloc(3 * sizeof(double)); + Multiply_Matrices(B, Q_1, 3, 3, U, 1); + B[0] = -B[0]; // x-axis combined bias + B[1] = -B[1]; // y-axis combined bias + B[2] = -B[2]; // z-axis combined bias + +// for(i = 0; i < 3; i++) + printf("\r\nCombined bias vector B:\r\n"); + printf("%8.2lf %8.2lf %8.2lf \r\n", B[0],B[1],B[2]); + + // First calculate QB = Q * B ( 3x1 = 3x3 * 3x1) + QB = (double*)malloc(3 * sizeof(double)); + Multiply_Matrices(QB, Q, 3, 3, B, 1); + + // Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1) + Multiply_Matrices(&btqb, B, 1, 3, QB, 1); + + // Calculate hmb = sqrt(btqb - J). + J = v[9]; + hmb = sqrt(btqb - J); + + // Calculate SQ, the square root of matrix Q + SSSS = (double*)malloc(3 * 3 * sizeof(double)); + Hessenberg_Form_Elementary(Q, SSSS, 3); + + eigen_real3 = (double*)malloc(3 * sizeof(double)); + eigen_imag3 = (double*)malloc(3 * sizeof(double)); + QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100); + + // normalize eigenvectors + norm1 = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]); + SSSS[0] /= norm1; + SSSS[3] /= norm1; + SSSS[6] /= norm1; + norm2 = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]); + SSSS[1] /= norm2; + SSSS[4] /= norm2; + SSSS[7] /= norm2; + norm3 = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]); + SSSS[2] /= norm3; + SSSS[5] /= norm3; + SSSS[8] /= norm3; + + Dz = (double*)malloc(3 * 3 * sizeof(double)); + for(i = 0; i < 9; i++) + Dz[i] = 0.0; + Dz[0] = sqrt(eigen_real3[0]); + Dz[4] = sqrt(eigen_real3[1]); + Dz[8] = sqrt(eigen_real3[2]); + + vdz = (double*)malloc(3 * 3 * sizeof(double)); + Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3); + + Transpose_Square_Matrix(SSSS, 3); + + SQ = (double*)malloc(3 * 3 * sizeof(double)); + Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3); + +// hm = 0.569; + A_1 = (double*)malloc(3 * 3 * sizeof(double)); + + for(i = 0; i < 9; i++) + A_1[i] = SQ[i] * hm / hmb; + + printf("\r\nCorrection matrix Ainv, using Hm=%8.1f:\r\n",hm); + for(i = 0; i < 3; i++) + printf("%9.5lf %9.5lf %9.5lf\r\n", A_1[i*3], A_1[i*3+1], A_1[i*3+2]); + printf("\r\nWhere Hcalc = Ainv*(H-B)\r\n"); + + printf("\r\nCode initialization statements...\r\n"); + printf("\r\n float B[3]\r\n {%8.2lf,%8.2lf,%8.2lf};\r\n",B[0],B[1],B[2]); + printf("\r\n float Ainv[3][3]\r\n {{%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[0],A_1[1],A_1[2]); + printf(" {%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[3],A_1[4],A_1[5]); + printf(" {%9.5lf,%9.5lf,%9.5lf}};\r\n",A_1[6],A_1[7],A_1[8]); + + + // dump corrected measurements if desired + + printf(" output filename for corrected values (csv) "); + scanf("%s",&filename); + if(strlen(filename) > 1) { + + fp2 = fopen(filename, "w"); + float xc,yc,zc; + xs=0; + for(i = 0; i // required for memcpy() + +void Copy_Vector(double *d, double *s, int n) +{ + memcpy(d, s, sizeof(double) * n); +} +//////////////////////////////////////////////////////////////////////////////// +// File: multiply_self_transpose.c // +// Routine(s): // +// Multiply_Self_Transpose // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Multiply_Self_Transpose(double *C, double *A, int nrows, int ncols ) // +// // +// Description: // +// Post multiply an nrows x ncols matrix A by its transpose. The result // +// is an nrows x nrows square symmetric matrix C, i.e. C = A A', where ' // +// denotes the transpose. // +// The matrix C should be declared as double C[nrows][nrows] in the // +// calling routine. The memory allocated to C should not include any // +// memory allocated to A. // +// // +// Arguments: // +// double *C Pointer to the first element of the matrix C. // +// double *A Pointer to the first element of the matrix A. // +// int nrows The number of rows of matrix A. // +// int ncols The number of columns of the matrices A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// double A[M][N], C[M][M]; // +// // +// (your code to initialize the matrix A) // +// // +// Multiply_Self_Transpose(&C[0][0], &A[0][0], M, N); // +// printf("The matrix C = AA ' is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Multiply_Self_Transpose(double *C, double *A, int nrows, int ncols) +{ + int i,j,k; + double *pA; + double *p_A = A; + double *pB; + double *pCdiag = C; + double *pC = C; + double *pCt; + + for (i = 0; i < nrows; pCdiag += nrows + 1, p_A = pA, i++) { + pC = pCdiag; + pCt = pCdiag; + pB = p_A; + for (j = i; j < nrows; pC++, pCt += nrows, j++) { + pA = p_A; + *pC = 0.0; + for (k = 0; k < ncols; k++) *pC += *(pA++) * *(pB++); + *pCt = *pC; + } + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: get_submatrix.c // +// Routine(s): // +// Get_Submatrix // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Get_Submatrix(double *S, int mrows, int mcols, // +// double *A, int ncols, int row, int col) // +// // +// Description: // +// Copy the mrows and mcols of the nrows x ncols matrix A starting with // +// A[row][col] to the submatrix S. // +// Note that S should be declared double S[mrows][mcols] in the calling // +// routine. // +// // +// Arguments: // +// double *S Destination address of the submatrix. // +// int mrows The number of rows of the matrix S. // +// int mcols The number of columns of the matrix S. // +// double *A Pointer to the first element of the matrix A[nrows][ncols]// +// int ncols The number of columns of the matrix A. // +// int row The row of A corresponding to the first row of S. // +// int col The column of A corresponding to the first column of S. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// #define NB // +// #define MB // +// double A[M][N], B[MB][NB]; // +// int row, col; // +// // +// (your code to set the matrix A, the row number row and column number // +// col) // +// // +// if ( (row >= 0) && (col >= 0) && ((row + MB) < M) && ((col + NB) < N) )// +// Get_Submatrix(&B[0][0], MB, NB, &A[0][0], N, row, col); // +// printf("The submatrix B is \n"); ... } // +//////////////////////////////////////////////////////////////////////////////// + +#include // required for memcpy() + +void Get_Submatrix(double *S, int mrows, int mcols, + double *A, int ncols, int row, int col) +{ + int number_of_bytes = sizeof(double) * mcols; + + for (A += row * ncols + col; mrows > 0; A += ncols, S+= mcols, mrows--) + memcpy(S, A, number_of_bytes); +} +//////////////////////////////////////////////////////////////////////////////// +// File: choleski.c // +// Contents: // +// Choleski_LU_Decomposition // +// Choleski_LU_Solve // +// Choleski_LU_Inverse // +// // +// Required Externally Defined Routines: // +// Lower_Triangular_Solve // +// Lower_Triangular_Inverse // +// Upper_Triangular_Solve // +//////////////////////////////////////////////////////////////////////////////// + +#include // required for sqrt() + +// Required Externally Defined Routines +int Lower_Triangular_Solve(double *L, double B[], double x[], int n); +int Lower_Triangular_Inverse(double *L, int n); +int Upper_Triangular_Solve(double *U, double B[], double x[], int n); + +//////////////////////////////////////////////////////////////////////////////// +// int Choleski_LU_Decomposition(double *A, int n) // +// // +// Description: // +// This routine uses Choleski's method to decompose the n x n positive // +// definite symmetric matrix A into the product of a lower triangular // +// matrix L and an upper triangular matrix U equal to the transpose of L. // +// The original matrix A is replaced by L and U with L stored in the // +// lower triangular part of A and the transpose U in the upper triangular // +// part of A. The original matrix A is therefore destroyed. // +// // +// Choleski's decomposition is performed by evaluating, in order, the // +// following pair of expressions for k = 0, ... ,n-1 : // +// L[k][k] = sqrt( A[k][k] - ( L[k][0] ^ 2 + ... + L[k][k-1] ^ 2 ) ) // +// L[i][k] = (A[i][k] - (L[i][0]*L[k][0] + ... + L[i][k-1]*L[k][k-1])) // +// / L[k][k] // +// and subsequently setting // +// U[k][i] = L[i][k], for i = k+1, ... , n-1. // +// // +// After performing the LU decomposition for A, call Choleski_LU_Solve // +// to solve the equation Ax = B or call Choleski_LU_Inverse to calculate // +// the inverse of A. // +// // +// Arguments: // +// double *A On input, the pointer to the first element of the matrix // +// A[n][n]. On output, the matrix A is replaced by the lower // +// and upper triangular Choleski factorizations of A. // +// int n The number of rows and/or columns of the matrix A. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix A is not positive definite symmetric (within // +// working accuracy). // +// // +// Example: // +// #define N // +// double A[N][N]; // +// // +// (your code to initialize the matrix A) // +// err = Choleski_LU_Decomposition((double *) A, N); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else { printf(" The LLt decomposition of A is \n"); // +// ... // +//////////////////////////////////////////////////////////////////////////////// +// // +int Choleski_LU_Decomposition(double *A, int n) +{ + int i, k, p; + double *p_Lk0; // pointer to L[k][0] + double *p_Lkp; // pointer to L[k][p] + double *p_Lkk; // pointer to diagonal element on row k. + double *p_Li0; // pointer to L[i][0] + double reciprocal; + + for (k = 0, p_Lk0 = A; k < n; p_Lk0 += n, k++) { + +// Update pointer to row k diagonal element. + + p_Lkk = p_Lk0 + k; + +// Calculate the difference of the diagonal element in row k +// from the sum of squares of elements row k from column 0 to +// column k-1. + + for (p = 0, p_Lkp = p_Lk0; p < k; p_Lkp += 1, p++) + *p_Lkk -= *p_Lkp * *p_Lkp; + +// If diagonal element is not positive, return the error code, +// the matrix is not positive definite symmetric. + + if ( *p_Lkk <= 0.0 ) return -1; + +// Otherwise take the square root of the diagonal element. + + *p_Lkk = sqrt( *p_Lkk ); + reciprocal = 1.0 / *p_Lkk; + +// For rows i = k+1 to n-1, column k, calculate the difference +// between the i,k th element and the inner product of the first +// k-1 columns of row i and row k, then divide the difference by +// the diagonal element in row k. +// Store the transposed element in the upper triangular matrix. + + p_Li0 = p_Lk0 + n; + for (i = k + 1; i < n; p_Li0 += n, i++) { + for (p = 0; p < k; p++) + *(p_Li0 + k) -= *(p_Li0 + p) * *(p_Lk0 + p); + *(p_Li0 + k) *= reciprocal; + *(p_Lk0 + i) = *(p_Li0 + k); + } + } + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Choleski_LU_Solve(double *LU, double *B, double *x, int n) // +// // +// Description: // +// This routine uses Choleski's method to solve the linear equation // +// Ax = B. This routine is called after the matrix A has been decomposed // +// into a product of a lower triangular matrix L and an upper triangular // +// matrix U which is the transpose of L. The matrix A is the product LU. // +// The solution proceeds by solving the linear equation Ly = B for y and // +// subsequently solving the linear equation Ux = y for x. // +// // +// Arguments: // +// double *LU Pointer to the first element of the matrix whose elements // +// form the lower and upper triangular matrix factors of A. // +// double *B Pointer to the column vector, (n x 1) matrix, B // +// double *x Solution to the equation Ax = B. // +// int n The number of rows and/or columns of the matrix LU. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Choleski_LU_Decomposition(&A[0][0], N); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else { // +// err = Choleski_LU_Solve(&A[0][0], B, x, n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The solution is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Choleski_LU_Solve(double *LU, double B[], double x[], int n) +{ + +// Solve the linear equation Ly = B for y, where L is a lower +// triangular matrix. + + if ( Lower_Triangular_Solve(LU, B, x, n) < 0 ) return -1; + +// Solve the linear equation Ux = y, where y is the solution +// obtained above of Ly = B and U is an upper triangular matrix. + + return Upper_Triangular_Solve(LU, x, x, n); +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Choleski_LU_Inverse(double *LU, int n) // +// // +// Description: // +// This routine uses Choleski's method to find the inverse of the matrix // +// A. This routine is called after the matrix A has been decomposed // +// into a product of a lower triangular matrix L and an upper triangular // +// matrix U which is the transpose of L. The matrix A is the product of // +// the L and U. Upon completion, the inverse of A is stored in LU so // +// that the matrix LU is destroyed. // +// // +// Arguments: // +// double *LU On input, the pointer to the first element of the matrix // +// whose elements form the lower and upper triangular matrix // +// factors of A. On output, the matrix LU is replaced by the // +// inverse of the matrix A equal to the product of L and U. // +// int n The number of rows and/or columns of the matrix LU. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Choleski_LU_Decomposition(&A[0][0], N); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else { // +// err = Choleski_LU_Inverse(&A[0][0], n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The inverse is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Choleski_LU_Inverse(double *LU, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + + if ( Lower_Triangular_Inverse(LU, n) < 0 ) return -1; + +// Premultiply L inverse by the transpose of L inverse. + + for (i = 0, p_i = LU; i < n; i++, p_i += n) { + for (j = 0, p_j = LU; j <= i; j++, p_j += n) { + sum = 0.0; + for (k = i, p_k = p_i; k < n; k++, p_k += n) + sum += *(p_k + i) * *(p_k + j); + *(p_i + j) = sum; + *(p_j + i) = sum; + } + } + + return 0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: multiply_matrices.c // +// Routine(s): // +// Multiply_Matrices // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Multiply_Matrices(double *C, double *A, int nrows, int ncols, // +// double *B, int mcols) // +// // +// Description: // +// Post multiply the nrows x ncols matrix A by the ncols x mcols matrix // +// B to form the nrows x mcols matrix C, i.e. C = A B. // +// The matrix C should be declared as double C[nrows][mcols] in the // +// calling routine. The memory allocated to C should not include any // +// memory allocated to A or B. // +// // +// Arguments: // +// double *C Pointer to the first element of the matrix C. // +// double *A Pointer to the first element of the matrix A. // +// int nrows The number of rows of the matrices A and C. // +// int ncols The number of columns of the matrices A and the // +// number of rows of the matrix B. // +// double *B Pointer to the first element of the matrix B. // +// int mcols The number of columns of the matrices B and C. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// #define NB // +// double A[M][N], B[N][NB], C[M][NB]; // +// // +// (your code to initialize the matrices A and B) // +// // +// Multiply_Matrices(&C[0][0], &A[0][0], M, N, &B[0][0], NB); // +// printf("The matrix C is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Multiply_Matrices(double *C, double *A, int nrows, int ncols, + double *B, int mcols) +{ + double *pB; + double *p_B; + int i,j,k; + + for (i = 0; i < nrows; A += ncols, i++) + for (p_B = B, j = 0; j < mcols; C++, p_B++, j++) { + pB = p_B; + *C = 0.0; + for (k = 0; k < ncols; pB += mcols, k++) + *C += *(A+k) * *pB; + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: identity_matrix.c // +// Routine(s): // +// Identity_Matrix // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Identity_Matrix(double *A, int n) // +// // +// Description: // +// Set the square n x n matrix A equal to the identity matrix, i.e. // +// A[i][j] = 0 if i != j and A[i][i] = 1. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int n The number of rows and columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// double A[N][N]; // +// // +// Identity_Matrix(&A[0][0], N); // +// printf("The matrix A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Identity_Matrix(double *A, int n) +{ + int i,j; + + for (i = 0; i < n - 1; i++) { + *A++ = 1.0; + for (j = 0; j < n; j++) *A++ = 0.0; + } + *A = 1.0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: hessenberg_elementary.c // +// Routine(s): // +// Hessenberg_Form_Elementary // +// Hessenberg_Elementary_Transform // +//////////////////////////////////////////////////////////////////////////////// + +#include // required for malloc() and free() +#include // required for fabs() + +// Required Externally Defined Routines +void Interchange_Rows(double *A, int row1, int row2, int ncols); +void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols); +void Identity_Matrix(double *A, int n); +void Copy_Vector(double *d, double *s, int n); + +// Internally Defined Routines +static void Hessenberg_Elementary_Transform(double* H, double *S, + int perm[], int n); + +//////////////////////////////////////////////////////////////////////////////// +// int Hessenberg_Form_Elementary(double *A, double *S, int n) // +// // +// Description: // +// This program transforms the square matrix A to a similar matrix in // +// Hessenberg form by a multiplying A on the right by a sequence of // +// elementary transformations and on the left by the sequence of inverse // +// transformations. // +// Def: Two matrices A and B are said to be similar if there exists a // +// nonsingular matrix S such that A S = S B. // +// Def A Hessenberg matrix is the sum of an upper triangular matrix and // +// a matrix all of whose components are 0 except possibly on its // +// subdiagonal. A Hessenberg matrix is sometimes said to be almost // +// upper triangular. // +// The algorithm proceeds by successivly selecting columns j = 0,...,n-3 // +// and then assuming that columns 0, ..., j-1 have been reduced to Hessen-// +// berg form, for rows j+1 to n-1, select that row j' for which |a[j'][j]|// +// is a maximum and interchange rows j+1 and j' and columns j+1 and j'. // +// Then for each i = j+2 to n-1, let x = a[i][j] / a[j+1][j] subtract // +// x * row j+1 from row i and add x * column i to column j+1. // +// // +// Arguments: // +// double *A On input a pointer to the first element of the matrix // +// A[n][n]. The matrix A is replaced with the matrix H, // +// a matrix in Hessenberg form similar to A. // +// double *S On output the transform such that A S = S H. // +// The matrix S should be dimensioned at least n x n in the // +// calling routine. // +// int n The number of rows or columns of the matrix A. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - Not enough memory // +// // +// Example: // +// #define N // +// double A[N][N], S[N][N]; // +// // +// (your code to create the matrix A) // +// if (Hessenberg_Form_Elementary(&A[0][0], (double*)S, N ) < 0) { // +// printf("Not enough memory\n"); exit(0); // +// } // +// // +//////////////////////////////////////////////////////////////////////////////// +// // +int Hessenberg_Form_Elementary(double *A, double* S, int n) +{ + int i, j, k, col, row; + int* perm; + double *p_row, *pS_row; + double max; + double s; + double *pA, *pB, *pC, *pS; + + // n x n matrices for which n <= 2 are already in Hessenberg form + + if (n <= 1) { *S = 1.0; return 0; } + if (n == 2) { *S++ = 1.0; *S++ = 0.0; *S++ = 1.0; *S = 0.0; return 0; } + + // Allocate working memory + + perm = (int*) malloc(n * sizeof(int)); + if (perm == NULL) return -1; // not enough memory + + // For each column use Elementary transformations + // to zero the entries below the subdiagonal. + + p_row = A + n; + pS_row = S + n; + for (col = 0; col < (n - 2); p_row += n, pS_row += n, col++) { + + // Find the row in column "col" with maximum magnitude where + // row >= col + 1. + + row = col + 1; + perm[row] = row; + for (pA = p_row + col, max = 0.0, i = row; i < n; pA += n, i++) + if (fabs(*pA) > max) { perm[row] = i; max = fabs(*pA); } + + // If perm[row] != row, then interchange row "row" and row + // perm[row] and interchange column "row" and column perm[row]. + + if ( perm[row] != row ) { + Interchange_Rows(A, row, perm[row], n); + Interchange_Columns(A, row, perm[row], n, n); + } + + // Zero out the components lying below the subdiagonal. + + pA = p_row + n; + pS = pS_row + n; + for (i = col + 2; i < n; pA += n, pS += n, i++) { + s = *(pA + col) / *(p_row + col); + for (j = 0; j < n; j++) + *(pA + j) -= *(p_row + j) * s; + *(pS + col) = s; + for (j = 0, pB = A + col + 1, pC = A + i; j < n; pB +=n, pC += n, j++) + *pB += s * *pC; + } + } + pA = A + n + n; + pS = S + n + n; + for (i = 2; i < n; pA += n, pS += n, i++) Copy_Vector(pA, pS, i - 1); + + Hessenberg_Elementary_Transform(A, S, perm, n); + + free(perm); + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Hessenberg_Elementary_Transform(double* H, double *S, // +// int perm[], int n) // +// // +// Description: // +// Given a n x n matrix A, let H be the matrix in Hessenberg form similar // +// to A, i.e. A S = S H. If v is an eigenvector of H with eigenvalue z, // +// i.e. Hv = zv, then ASv = SHv = z Sv, i.e. Sv is the eigenvector of A // +// with corresponding eigenvalue z. // +// This routine returns S where S is the similarity transformation such // +// that A S = S H. // +// // +// Arguments: // +// double* H On input a matrix in Hessenberg form with transformation // +// elements stored below the subdiagonal part. // +// On output the matrix in Hessenberg form with elements // +// below the subdiagonal zeroed out. // +// double* S On output, the transformations matrix such that // +// A S = S H. // +// int perm[] Array of row/column interchanges. // +// int n The order of the matrices H and S. // +// // +// Return Values: // +// void // +// // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Hessenberg_Elementary_Transform(double *H, double* S, int perm[], + int n) +{ + int i, j; + double *pS, *pH; + double x; + + Identity_Matrix(S, n); + for (i = n - 2; i >= 1; i--) { + pH = H + n * (i + 1); + pS = S + n * (i + 1); + for (j = i + 1; j < n; pH += n, pS += n, j++) { + *(pS + i) = *(pH + i - 1); + *(pH + i - 1) = 0.0; + } + if (perm[i] != i) { + pS = S + n * i; + pH = S + n * perm[i]; + for (j = i; j < n; j++) { + *(pS + j) = *(pH + j); + *(pH + j) = 0.0; + } + *(pH + i) = 1.0; + } + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: qr_hessenberg_matrix.c // +// Routine(s): // +// QR_Hessenberg_Matrix // +//////////////////////////////////////////////////////////////////////////////// + +#include // required for fabs() and sqrt() +#include // required for DBL_EPSILON + + +// Internally Defined Routines +static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], + double eigen_imag[], int row, double shift); +static void Two_Eigenvalues(double *H, double *S, double eigen_real[], + double eigen_imag[], int n, int k, double t); +static void Update_Row(double *Hrow, double cos, double sin, int n, int k); +static void Update_Column(double* H, double cos, double sin, int n, int k); +static void Update_Transformation(double *S, double cos, double sin, + int n, int k); +static void Double_QR_Iteration(double *H, double *S, int row, int min_row, + int n, double* shift, int iteration); +static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, + double* shift, double *trace, double *det, int iteration); +static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, + int max_row, int n, double trace, double det); +static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, + double trace, double det, double *S, int n); +static void Complex_Division(double x, double y, double u, double v, + double* a, double* b); +static void BackSubstitution(double *H, double eigen_real[], + double eigen_imag[], int n); +static void BackSubstitute_Real_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n); +static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n); +static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], + double eigen_imag[], int n); + +//////////////////////////////////////////////////////////////////////////////// +// int QR_Hessenberg_Matrix( double *H, double *S, double eigen_real[], // +// double eigen_imag[], int n, int max_iteration_count) // +// // +// Description: // +// This program calculates the eigenvalues and eigenvectors of a matrix // +// in Hessenberg form. This routine is adapted from the routine 'hql2' // +// appearing in 'Handbook for Automatic Computation, vol 2: Linear // +// Algebra' published by Springer Verlag (1971) edited by Wilkinson and // +// Reinsch, Contribution II/15 Eigenvectors of Real and Complex Matrices // +// by LR and QR Triangularizations by Peters and Wilkinson. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the real n x n matrix H in upper// +// Hessenberg form. // +// double *S // +// If H is the primary data matrix, the matrix S should be set // +// to the identity n x n identity matrix on input. If H is // +// derived from an n x n matrix A, then S should be the // +// transformation matrix such that AS = SH. On output, the i-th // +// column of S corresponds to the i-th eigenvalue if that eigen- // +// value is real and the i-th and (i+1)-st columns of S correspond // +// to the i-th eigenvector with the real part in the i-th column // +// and positive imaginary part in the (i+1)-st column if that // +// eigenvalue is complex with positive imaginary part. The // +// eigenvector corresponding to the eigenvalue with negative // +// imaginary part is the complex conjugate of the eigenvector // +// corresponding to the complex conjugate of the eigenvalue. // +// If on input, S was the identity matrix, then the columns are // +// the eigenvectors of H as described, if S was a transformation // +// matrix so that AS = SH, then the columns of S are the // +// eigenvectors of A as described. // +// double eigen_real[] // +// Upon return, eigen_real[i] will contain the real part of the // +// i-th eigenvalue. // +// double eigen_imag[] // +// Upon return, eigen_ima[i] will contain the imaginary part of // +// the i-th eigenvalue. // +// int n // +// The number of rows or columns of the upper Hessenberg matrix A. // +// int max_iteration_count // +// The maximum number of iterations to try to find an eigenvalue // +// before quitting. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - Unable to find an eigenvalue within 'max_iteration_count' // +// iterations. // +// // +// Example: // +// #define N // +// #define MAX_ITERATION_COUNT // +// double H[N][N], S[N][N], eigen_real[N], eigen_imag[N]; // +// int k; // +// // +// (code to initialize H[N][N] and S[N][N]) // +// k = QR_Hessenberg_Matrix( (double*)H, (double*)S, eigen_real, // +// eigen_imag, N, MAX_ITERATION_COUNT); // +// if (k < 0) {printf("Failed"); exit(1);} // +//////////////////////////////////////////////////////////////////////////////// +// // +int QR_Hessenberg_Matrix( double *H, double *S, double eigen_real[], + double eigen_imag[], int n, int max_iteration_count) +{ + int i; + int row; + int iteration; + int found_eigenvalue; + double shift = 0.0; + double* pH; + + for ( row = n - 1; row >= 0; row--) { + found_eigenvalue = 0; + for (iteration = 1; iteration <= max_iteration_count; iteration++) { + + // Search for small subdiagonal element + + for (i = row, pH = H + row * n; i > 0; i--, pH -= n) + if (fabs(*(pH + i - 1 )) <= DBL_EPSILON * + ( fabs(*(pH - n + i - 1)) + fabs(*(pH + i)) ) ) break; + + // If the subdiagonal element on row "row" is small, then + // that row element is an eigenvalue. If the subdiagonal + // element on row "row-1" is small, then the eigenvalues + // of the 2x2 diagonal block consisting rows "row-1" and + // "row" are eigenvalues. Otherwise perform a double QR + // iteration. + + switch(row - i) { + case 0: // One real eigenvalue + One_Real_Eigenvalue(pH, eigen_real, eigen_imag, i, shift); + found_eigenvalue = 1; + break; + case 1: // Either two real eigenvalues or a complex pair + row--; + Two_Eigenvalues(H, S, eigen_real, eigen_imag, n, row, shift); + found_eigenvalue = 1; + break; + default: + Double_QR_Iteration(H, S, i, row, n, &shift, iteration); + } + if (found_eigenvalue) break; + } + if (iteration > max_iteration_count) return -1; + } + + BackSubstitution(H, eigen_real, eigen_imag, n); + Calculate_Eigenvectors(H, S, eigen_real, eigen_imag, n); + + return 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// static void One_Real_Eigenvalue( double Hrow[], double eigen_real[], // +// double eigen_imag[], int row, double shift) // +// // +// Arguments: // +// double Hrow[] // +// Pointer to the row "row" of the matrix in Hessenberg form. // +// double eigen_real[] // +// Array of the real parts of the eigenvalues. // +// double eigen_imag[] // +// Array of the imaginary parts of the eigenvalues. // +// int row // +// The row to which the pointer Hrow[] points of the matrix H. // +// double shift // +// The cumulative exceptional shift of the diagonal elements of // +// the matrix H. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], + double eigen_imag[], int row, double shift) +{ + Hrow[row] += shift; + eigen_real[row] = Hrow[row]; + eigen_imag[row] = 0.0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Two_Eigenvalues( double *H, double *S, double eigen_real[], // +// double eigen_imag[], int n, int row, double shift) // +// // +// Description: // +// Given the 2x2 matrix A = (a[i][j]), the characteristic equation is: // +// x^2 - Tr(A) x + Det(A) = 0, where Tr(A) = a[0][0] + a[1][1] and // +// Det(A) = a[0][0] * a[1][1] - a[0][1] * a[1][0]. // +// The solution for the eigenvalues x are: // +// x1 = (Tr(A) + sqrt( (Tr(A))^2 + 4 * a[0][1] * a[1][0] ) / 2 and // +// x2 = (Tr(A) - sqrt( (Tr(A))^2 + 4 * a[0][1] * a[1][0] ) / 2. // +// Let p = (a[0][0] - a[1][1]) / 2 and q = p^2 - a[0][1] * a[1][0], then // +// x1 = a[1][1] + p [+|-] sqrt(q) and x2 = a[0][0] + a[1][1] - x1. // +// Choose the sign [+|-] to be the sign of p. // +// If q > 0.0, then both roots are real and the transformation // +// | cos sin | | a[0][0] a[0][1] | | cos -sin | // +// |-sin cos | | a[1][0] a[1][1] | | sin cos | // +// where sin = a[1][0] / r, cos = ( p + sqrt(q) ) / r, where r > 0 is // +// determined sin^2 + cos^2 = 1 transforms the matrix A to an upper // +// triangular matrix with x1 the upper diagonal element and x2 the lower.// +// If q < 0.0, then both roots form a complex conjugate pair. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double *S // +// Pointer to the first element of the transformation matrix. // +// double eigen_real[] // +// Array of the real parts of the eigenvalues. // +// double eigen_imag[] // +// Array of the imaginary parts of the eigenvalues. // +// int n // +// The dimensions of the matrix H and S. // +// int row // +// The upper most row of the block diagonal 2 x 2 submatrix of H. // +// double shift // +// The cumulative exceptional shift of the diagonal elements of // +// the matrix H. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Two_Eigenvalues(double *H, double* S, double eigen_real[], + double eigen_imag[], int n, int row, double shift) +{ + double p, q, x, discriminant, r; + double cos, sin; + double *Hrow = H + n * row; + double *Hnextrow = Hrow + n; + int nextrow = row + 1; + + p = 0.5 * (Hrow[row] - Hnextrow[nextrow]); + x = Hrow[nextrow] * Hnextrow[row]; + discriminant = p * p + x; + Hrow[row] += shift; + Hnextrow[nextrow] += shift; + if (discriminant > 0.0) { // pair of real roots + q = sqrt(discriminant); + if (p < 0.0) q = p - q; else q += p; + eigen_real[row] = Hnextrow[nextrow] + q; + eigen_real[nextrow] = Hnextrow[nextrow] - x / q; + eigen_imag[row] = 0.0; + eigen_imag[nextrow] = 0.0; + r = sqrt(Hnextrow[row]*Hnextrow[row] + q * q); + sin = Hnextrow[row] / r; + cos = q / r; + Update_Row(Hrow, cos, sin, n, row); + Update_Column(H, cos, sin, n, row); + Update_Transformation(S, cos, sin, n, row); + } + else { // pair of complex roots + eigen_real[nextrow] = eigen_real[row] = Hnextrow[nextrow] + p; + eigen_imag[row] = sqrt(fabs(discriminant)); + eigen_imag[nextrow] = -eigen_imag[row]; + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Update_Row(double *Hrow, double cos, double sin, int n, // +// int row) // +// // +// Description: // +// Update rows 'row' and 'row + 1' using the rotation matrix: // +// | cos sin | // +// |-sin cos |. // +// I.e. multiply the matrix H on the left by the identity matrix with // +// the 2x2 diagonal block starting at row 'row' replaced by the above // +// 2x2 rotation matrix. // +// // +// Arguments: // +// double Hrow[] // +// Pointer to the row "row" of the matrix in Hessenberg form. // +// double cos // +// Cosine of the rotation angle. // +// double sin // +// Sine of the rotation angle. // +// int n // +// The dimension of the matrix H. // +// int row // +// The row to which the pointer Hrow[] points of the matrix H // +// in Hessenberg form. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Update_Row(double *Hrow, double cos, double sin, int n, int row) +{ + double x; + double *Hnextrow = Hrow + n; + int i; + + for (i = row; i < n; i++) { + x = Hrow[i]; + Hrow[i] = cos * x + sin * Hnextrow[i]; + Hnextrow[i] = cos * Hnextrow[i] - sin * x; + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Update_Column(double* H, double cos, double sin, int n, // +// int col) // +// // +// Description: // +// Update columns 'col' and 'col + 1' using the rotation matrix: // +// | cos -sin | // +// | sin cos |. // +// I.e. multiply the matrix H on the right by the identity matrix with // +// the 2x2 diagonal block starting at row 'col' replaced by the above // +// 2x2 rotation matrix. // +// // +// Arguments: // +// double *H // +// Pointer to the matrix in Hessenberg form. // +// double cos // +// Cosine of the rotation angle. // +// double sin // +// Sine of the rotation angle. // +// int n // +// The dimension of the matrix H. // +// int col // +// The left-most column of the matrix H to update. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Update_Column(double* H, double cos, double sin, int n, int col) +{ + double x; + int i; + int next_col = col + 1; + + for (i = 0; i <= next_col; i++, H += n) { + x = H[col]; + H[col] = cos * x + sin * H[next_col]; + H[next_col] = cos * H[next_col] - sin * x; + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Update_Transformation(double *S, double cos, double sin, // +// int n, int k) // +// // +// Description: // +// Update columns 'k' and 'k + 1' using the rotation matrix: // +// | cos -sin | // +// | sin cos |. // +// I.e. multiply the matrix S on the right by the identity matrix with // +// the 2x2 diagonal block starting at row 'k' replaced by the above // +// 2x2 rotation matrix. // +// // +// Arguments: // +// double *S // +// Pointer to the row "row" of the matrix in Hessenberg form. // +// double cos // +// Pointer to the first element of the matrix in Hessenberg form. // +// double sin // +// Pointer to the first element of the transformation matrix. // +// int n // +// The dimensions of the matrix H and S. // +// int k // +// The row to which the pointer Hrow[] points of the matrix H. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Update_Transformation(double *S, double cos, double sin, + int n, int k) +{ + double x; + int i; + int k1 = k + 1; + + for (i = 0; i < n; i++, S += n) { + x = S[k]; + S[k] = cos * x + sin * S[k1]; + S[k1] = cos * S[k1] - sin * x; + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Double_QR_Iteration(double *H, double *S, int min_row, // +// int max_row, int n, double* shift, int iteration) // +// // +// Description: // +// Calculate the trace and determinant of the 2x2 matrix: // +// | H[k-1][k-1] H[k-1][k] | // +// | H[k][k-1] H[k][k] | // +// unless iteration = 0 (mod 10) in which case increment the shift and // +// decrement the first k elements of the matrix H, then fudge the trace // +// and determinant by trace = 3/2( |H[k][k-1]| + |H[k-1][k-2]| and // +// det = 4/9 trace^2. // +// // +// Arguments: // +// double *H // +// Pointer to the matrix H in Hessenberg form. // +// double *S // +// Pointer to the transformation matrix S. // +// int min_row // +// The top-most row in which the off-diagonal element of H is // +// negligible. If no such row exists, then min_row = 0. // +// int max_row // +// The maximum row of the block 2 x 2 diagonal matrix used to // +// estimate the two eigenvalues for the two implicit shifts. // +// int n // +// The dimensions of the matrix H and S. // +// double *shift // +// The cumulative exceptional shift of the diagonal elements of // +// the matrix H. // +// int iteration // +// Current iteration count. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Double_QR_Iteration(double *H, double *S, int min_row, int max_row, + int n, double* shift, int iteration) +{ + int k; + double trace, det; + + Product_and_Sum_of_Shifts(H, n, max_row, shift, &trace, &det, iteration); + k = Two_Consecutive_Small_Subdiagonal(H, min_row, max_row, n, trace, det); + Double_QR_Step(H, min_row, max_row, k, trace, det, S, n); +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, // +// double* shift, double *trace, double *det, int iteration) // +// // +// Description: // +// Calculate the trace and determinant of the 2x2 matrix: // +// | H[k-1][k-1] H[k-1][k] | // +// | H[k][k-1] H[k][k] | // +// unless iteration = 0 (mod 10) in which case increment the shift and // +// decrement the first k elements of the matrix H, then fudge the trace // +// and determinant by trace = 3/2( |H[k][k-1]| + |H[k-1][k-2]| and // +// det = 4/9 trace^2. // +// // +// Arguments: // +// double *H // +// Pointer to the matrix H in Hessenberg form. // +// int n // +// The dimension of the matrix H. // +// int max_row // +// The maximum row of the block 2 x 2 diagonal matrix used to // +// estimate the two eigenvalues for the two implicit shifts. // +// double *shift // +// The cumulative exceptional shift of the diagonal elements of // +// the matrix H. Modified if an exceptional shift occurs. // +// double *trace // +// Returns the trace of the 2 x 2 block diagonal matrix starting // +// at the row/column max_row-1. For an exceptional shift, the // +// trace is set as described above. // +// double *det // +// Returns the determinant of the 2 x 2 block diagonal matrix // +// starting at the row/column max_row-1. For an exceptional shift,// +// the determinant is set as described above. // +// int iteration // +// Current iteration count. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, + double* shift, double *trace, double *det, int iteration) +{ + double *pH = H + max_row * n; + double *p_aux; + int i; + int min_col = max_row - 1; + + if ( ( iteration % 10 ) == 0 ) { + *shift += pH[max_row]; + for (i = 0, p_aux = H; i <= max_row; p_aux += n, i++) + p_aux[i] -= pH[max_row]; + p_aux = pH - n; + *trace = fabs(pH[min_col]) + fabs(p_aux[min_col - 1]); + *det = *trace * *trace; + *trace *= 1.5; + } + else { + p_aux = pH - n; + *trace = p_aux[min_col] + pH[max_row]; + *det = p_aux[min_col] * pH[max_row] - p_aux[max_row] * pH[min_col]; + } +}; + + +//////////////////////////////////////////////////////////////////////////////// +// static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, // +// int max_row, int n, double trace, double det) // +// // +// Description: // +// To reduce the amount of computation in Francis' double QR step search // +// for two consecutive small subdiagonal elements from row nn to row m, // +// where m < nn. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// int min_row // +// The row in which to end the search (search is from upwards). // +// int max_row // +// The row in which to begin the search. // +// int n // +// The dimension of H. // +// double trace // +// The trace of the lower 2 x 2 block diagonal matrix. // +// double det // +// The determinant of the lower 2 x 2 block diagonal matrix. // +// // +// Return Value: // +// Row with negligible subdiagonal element or min_row if none found. // +//////////////////////////////////////////////////////////////////////////////// +// // +static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, + int max_row, int n, double trace, double det) +{ + double x, y ,z, s; + double* pH; + int i, k; + + for (k = max_row - 2, pH = H + k * n; k >= min_row; pH -= n, k--) { + x = (pH[k] * ( pH[k] - trace ) + det) / pH[n+k] + pH[k+1]; + y = pH[k] + pH[n+k+1] - trace; + z = pH[n + n + k + 1]; + s = fabs(x) + fabs(y) + fabs(z); + x /= s; + y /= s; + z /= s; + if (k == min_row) break; + if ( (fabs(pH[k-1]) * (fabs(y) + fabs(z)) ) <= + DBL_EPSILON * fabs(x) * + (fabs(pH[k-1-n]) + fabs(pH[k]) + fabs(pH[n + k + 1])) ) break; + } + for (i = k+2, pH = H + i * n; i <= max_row; pH += n, i++) pH[i-2] = 0.0; + for (i = k+3, pH = H + i * n; i <= max_row; pH += n, i++) pH[i-3] = 0.0; + return k; +}; + + +//////////////////////////////////////////////////////////////////////////////// +// static void Double_QR_Step(double *H, int min_row, int max_row, // +// int min_col, double *S, int n) // +// // +// Description: // +// Perform Francis' double QR step from rows 'min_row' to 'max_row' // +// and columns 'min_col' to 'max_row'. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// int min_row // +// The row in which to begin. // +// int max_row // +// The row in which to end. // +// int min_col // +// The column in which to begin. // +// double trace // +// The trace of the lower 2 x 2 block diagonal matrix. // +// double det // +// The determinant of the lower 2 x 2 block diagonal matrix. // +// double *S // +// Pointer to the first element of the transformation matrix. // +// int n // +// The dimensions of H and S. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, + double trace, double det, double *S, int n) +{ + double s, x, y, z; + double a, b, c; + double *pH; + double *tH; + double *pS; + int i,j,k; + int last_test_row_col = max_row - 1; + + k = min_col; + pH = H + min_col * n; + a = (pH[k] * ( pH[k] - trace ) + det) / pH[n+k] + pH[k+1]; + b = pH[k] + pH[n+k+1] - trace; + c = pH[n + n + k + 1]; + s = fabs(a) + fabs(b) + fabs(c); + a /= s; + b /= s; + c /= s; + + for (; k <= last_test_row_col; k++, pH += n) { + if ( k > min_col ) { + c = (k == last_test_row_col) ? 0.0 : pH[n + n + k - 1]; + x = fabs(pH[k-1]) + fabs(pH[n + k - 1]) + fabs(c); + if ( x == 0.0 ) continue; + a = pH[k - 1] / x; + b = pH[n + k - 1] / x; + c /= x; + } + s = sqrt( a * a + b * b + c * c ); + if (a < 0.0) s = -s; + if ( k > min_col ) pH[k-1] = -s * x; + else if (min_row != min_col) pH[k-1] = -pH[k-1]; + a += s; + x = a / s; + y = b / s; + z = c / s; + b /= a; + c /= a; + + // Update rows k, k+1, k+2 + for (j = k; j < n; j++) { + a = pH[j] + b * pH[n+j]; + if ( k != last_test_row_col ) { + a += c * pH[n + n + j]; + pH[n + n + j] -= a * z; + } + pH[n + j] -= a * y; + pH[j] -= a * x; + } + + // Update column k+1 + + j = k + 3; + if (j > max_row) j = max_row; + for (i = 0, tH = H; i <= j; i++, tH += n) { + a = x * tH[k] + y * tH[k+1]; + if ( k != last_test_row_col ) { + a += z * tH[k+2]; + tH[k+2] -= a * c; + } + tH[k+1] -= a * b; + tH[k] -= a; + } + + // Update transformation matrix + + for (i = 0, pS = S; i < n; pS += n, i++) { + a = x * pS[k] + y * pS[k+1]; + if ( k != last_test_row_col ) { + a += z * pS[k+2]; + pS[k+2] -= a * c; + } + pS[k+1] -= a * b; + pS[k] -= a; + } + }; +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitution(double *H, double eigen_real[], // +// double eigen_imag[], int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitution(double *H, double eigen_real[], + double eigen_imag[], int n) +{ + double zero_tolerance; + double *pH; + int i, j, row; + + // Calculate the zero tolerance + + pH = H; + zero_tolerance = fabs(pH[0]); + for (pH += n, i = 1; i < n; pH += n, i++) + for (j = i-1; j < n; j++) zero_tolerance += fabs(pH[j]); + zero_tolerance *= DBL_EPSILON; + + // Start Backsubstitution + + for (row = n-1; row >= 0; row--) { + if (eigen_imag[row] == 0.0) + BackSubstitute_Real_Vector(H, eigen_real, eigen_imag, row, + zero_tolerance, n); + else if ( eigen_imag[row] < 0.0 ) + BackSubstitute_Complex_Vector(H, eigen_real, eigen_imag, row, + zero_tolerance, n); + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitute_Real_Vector(double *H, double eigen_real[], // +// double eigen_imag[], int row, double zero_tolerance, int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int row // +// double zero_tolerance // +// Zero substitute. To avoid dividing by zero. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitute_Real_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n) +{ + double *pH; + double *pV; + double x,y; + double u[4]; + double v[2]; + int i,j,k; + + k = row; + pH = H + row * n; + pH[row] = 1.0; + for (i = row - 1, pH -= n; i >= 0; i--, pH -= n) { + u[0] = pH[i] - eigen_real[row]; + v[0] = pH[row]; + pV = H + n * k; + for (j = k; j < row; j++, pV += n) v[0] += pH[j] * pV[row]; + if ( eigen_imag[i] < 0.0 ) { + u[3] = u[0]; + v[1] = v[0]; + } else { + k = i; + if (eigen_imag[i] == 0.0) { + if (u[0] != 0.0) pH[row] = - v[0] / u[0]; + else pH[row] = - v[0] / zero_tolerance; + } else { + u[1] = pH[i+1]; + u[2] = pH[n+i]; + x = (eigen_real[i] - eigen_real[row]); + x *= x; + x += eigen_imag[i] * eigen_imag[i]; + pH[row] = (u[1] * v[1] - u[3] * v[0]) / x; + if ( fabs(u[1]) > fabs(u[3]) ) + pH[n+row] = -(v[0] + u[0] * pH[row]) / u[1]; + else + pH[n+row] = -(v[1] + u[2] * pH[row]) / u[3]; + } + } + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], // +// double eigen_imag[], int row, double zero_tolerance, int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int row // +// double zero_tolerance // +// Zero substitute. To avoid dividing by zero. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n) +{ + double *pH; + double *pV; + double x,y; + double u[4]; + double v[2]; + double w[2]; + int i,j,k; + + k = row - 1; + pH = H + n * row; + if ( fabs(pH[k]) > fabs(pH[row-n]) ) { + pH[k-n] = - (pH[row] - eigen_real[row]) / pH[k]; + pH[row-n] = -eigen_imag[row] / pH[k]; + } + else + Complex_Division(-pH[row-n], 0.0, + pH[k-n]-eigen_real[row], eigen_imag[row], &pH[k-n], &pH[row-n]); + pH[k] = 1.0; + pH[row] = 0.0; + for (i = row - 2, pH = H + n * i; i >= 0; pH -= n, i--) { + u[0] = pH[i] - eigen_real[row]; + w[0] = pH[row]; + w[1] = 0.0; + pV = H + k * n; + for (j = k; j < row; j++, pV+=n) { + w[0] += pH[j] * pV[row - 1]; + w[1] += pH[j] * pV[row]; + } + if (eigen_imag[i] < 0.0) { + u[3] = u[0]; + v[0] = w[0]; + v[1] = w[1]; + } else { + k = i; + if (eigen_imag[i] == 0.0) { + Complex_Division(-w[0], -w[1], u[0], eigen_imag[row], &pH[row-1], + &pH[row]); + } + else { + u[1] = pH[i+1]; + u[2] = pH[n + i]; + x = eigen_real[i] - eigen_real[row]; + y = 2.0 * x * eigen_imag[row]; + x = x * x + eigen_imag[i] * eigen_imag[i] + - eigen_imag[row] * eigen_imag[row]; + if ( x == 0.0 && y == 0.0 ) + x = zero_tolerance * ( fabs(u[0]) + fabs(u[1]) + fabs(u[2]) + + fabs(u[3]) + fabs(eigen_imag[row]) ); + Complex_Division(u[1]*v[0] - u[3] * w[0] + w[1] * eigen_imag[row], + u[1] * v[1] - u[3] * w[1] - w[0] * eigen_imag[row], + x, y, &pH[row-1], &pH[row]); + if ( fabs(u[1]) > (fabs(u[3]) + fabs(eigen_imag[row])) ) { + pH[n+row-1] = -w[0] - u[0] * pH[row-1] + + eigen_imag[row] * pH[row] / u[1]; + pH[n+row] = -w[1] - u[0] * pH[row] + - eigen_imag[row] * pH[row-1] / u[1]; + } + else { + Complex_Division(-v[0] - u[2] * pH[row-1], -v[1] - u[2]*pH[row], + u[3], eigen_imag[row], &pH[n+row-1], &pH[n+row]); + } + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// static void Calculate_Eigenvectors(double *H, double *S, // +// double eigen_real[], double eigen_imag[], int n) // +// // +// Description: // +// Multiply by transformation matrix. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double *S // +// Pointer to the first element of the transformation matrix. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int n // +// The dimension of H, S, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], + double eigen_imag[], int n) +{ + double* pH; + double* pS; + double x,y; + int i,j,k; + + for (k = n-1; k >= 0; k--) { + if (eigen_imag[k] < 0.0) { + for (i = 0, pS = S; i < n; pS += n, i++) { + x = 0.0; + y = 0.0; + for (j = 0, pH = H; j <= k; pH += n, j++) { + x += pS[j] * pH[k-1]; + y += pS[j] * pH[k]; + } + pS[k-1] = x; + pS[k] = y; + } + } else if (eigen_imag[k] == 0.0) { + for (i = 0, pS = S; i < n; i++, pS += n) { + x = 0.0; + for (j = 0, pH = H; j <= k; j++, pH += n) + x += pS[j] * pH[k]; + pS[k] = x; + } + } + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Complex_Division(double x, double y, double u, double v, // +// double* a, double* b) // +// // +// Description: // +// a + i b = (x + iy) / (u + iv) // +// = (x * u + y * v) / r^2 + i (y * u - x * v) / r^2, // +// where r^2 = u^2 + v^2. // +// // +// Arguments: // +// double x // +// Real part of the numerator. // +// double y // +// Imaginary part of the numerator. // +// double u // +// Real part of the denominator. // +// double v // +// Imaginary part of the denominator. // +// double *a // +// Real part of the quotient. // +// double *b // +// Imaginary part of the quotient. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Complex_Division(double x, double y, double u, double v, + double* a, double* b) +{ + double q = u*u + v*v; + + *a = (x * u + y * v) / q; + *b = (y * u - x * v) / q; +} +//////////////////////////////////////////////////////////////////////////////// +// File: transpose_square_matrix.c // +// Routine(s): // +// Transpose_Square_Matrix // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Transpose_Square_Matrix( double *A, int n ) // +// // +// Description: // +// Take the transpose of A and store in place. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int n The number of rows and columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// double A[N][N]; // +// // +// (your code to initialize the matrix A) // +// // +// Transpose_Square_Matrix( &A[0][0], N); // +// printf("The transpose of A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Transpose_Square_Matrix( double *A, int n ) +{ + double *pA, *pAt; + double temp; + int i,j; + + for (i = 0; i < n; A += n + 1, i++) { + pA = A + 1; + pAt = A + n; + for (j = i+1 ; j < n; pA++, pAt += n, j++) { + temp = *pAt; + *pAt = *pA; + *pA = temp; + } + } +} +/////////////////////////////////////////////////////////////////////////////// +// File: lower_triangular.c // +// Routines: // +// Lower_Triangular_Solve // +// Lower_Triangular_Inverse // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// int Lower_Triangular_Solve(double *L, double *B, double x[], int n) // +// // +// Description: // +// This routine solves the linear equation Lx = B, where L is an n x n // +// lower triangular matrix. (The superdiagonal part of the matrix is // +// not addressed.) // +// The algorithm follows: // +// x[0] = B[0]/L[0][0], and // +// x[i] = [B[i] - (L[i][0] * x[0] + ... + L[i][i-1] * x[i-1])] / L[i][i],// +// for i = 1, ..., n-1. // +// // +// Arguments: // +// double *L Pointer to the first element of the lower triangular // +// matrix. // +// double *B Pointer to the column vector, (n x 1) matrix, B. // +// double *x Pointer to the column vector, (n x 1) matrix, x. // +// int n The number of rows or columns of the matrix L. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Lower_Triangular_Solve(&A[0][0], B, x, n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The solution is \n"); // +// ... // +//////////////////////////////////////////////////////////////////////////////// +// // +int Lower_Triangular_Solve(double *L, double B[], double x[], int n) +{ + int i, k; + +// Solve the linear equation Lx = B for x, where L is a lower +// triangular matrix. + + for (k = 0; k < n; L += n, k++) { + if (*(L + k) == 0.0) return -1; // The matrix L is singular + x[k] = B[k]; + for (i = 0; i < k; i++) x[k] -= x[i] * *(L + i); + x[k] /= *(L + k); + } + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Lower_Triangular_Inverse(double *L, int n) // +// // +// Description: // +// This routine calculates the inverse of the lower triangular matrix L. // +// The superdiagonal part of the matrix is not addressed. // +// The algorithm follows: // +// Let M be the inverse of L, then L M = I, // +// M[i][i] = 1.0 / L[i][i] for i = 0, ..., n-1, and // +// M[i][j] = -[(L[i][j] M[j][j] + ... + L[i][i-1] M[i-1][j])] / L[i][i], // +// for i = 1, ..., n-1, j = 0, ..., i - 1. // +// // +// // +// Arguments: // +// double *L On input, the pointer to the first element of the matrix // +// whose lower triangular elements form the matrix which is // +// to be inverted. On output, the lower triangular part is // +// replaced by the inverse. The superdiagonal elements are // +// not modified. // +// its inverse. // +// int n The number of rows and/or columns of the matrix L. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double L[N][N]; // +// // +// (your code to create the matrix L) // +// err = Lower_Triangular_Inverse(&L[0][0], N); // +// if (err < 0) printf(" Matrix L is singular\n"); // +// else { // +// printf(" The inverse is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Lower_Triangular_Inverse(double *L, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + +// Invert the diagonal elements of the lower triangular matrix L. + + for (k = 0, p_k = L; k < n; p_k += (n + 1), k++) { + if (*p_k == 0.0) return -1; + else *p_k = 1.0 / *p_k; + } + +// Invert the remaining lower triangular matrix L row by row. + + for (i = 1, p_i = L + n; i < n; i++, p_i += n) { + for (j = 0, p_j = L; j < i; p_j += n, j++) { + sum = 0.0; + for (k = j, p_k = p_j; k < i; k++, p_k += n) + sum += *(p_i + k) * *(p_k + j); + *(p_i + j) = - *(p_i + i) * sum; + } + } + + return 0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: upper_triangular.c // +// Routines: // +// Upper_Triangular_Solve // +// Upper_Triangular_Inverse // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// int Upper_Triangular_Solve(double *U, double *B, double x[], int n) // +// // +// Description: // +// This routine solves the linear equation Ux = B, where U is an n x n // +// upper triangular matrix. (The subdiagonal part of the matrix is // +// not addressed.) // +// The algorithm follows: // +// x[n-1] = B[n-1]/U[n-1][n-1], and // +// x[i] = [B[i] - (U[i][i+1] * x[i+1] + ... + U[i][n-1] * x[n-1])] // +// / U[i][i], // +// for i = n-2, ..., 0. // +// // +// Arguments: // +// double *U Pointer to the first element of the upper triangular // +// matrix. // +// double *B Pointer to the column vector, (n x 1) matrix, B. // +// double *x Pointer to the column vector, (n x 1) matrix, x. // +// int n The number of rows or columns of the matrix U. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix U is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Upper_Triangular_Solve(&A[0][0], B, x, n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The solution is \n"); // +// ... // +//////////////////////////////////////////////////////////////////////////////// +// // +int Upper_Triangular_Solve(double *U, double B[], double x[], int n) +{ + int i, k; + +// Solve the linear equation Ux = B for x, where U is an upper +// triangular matrix. + + for (k = n-1, U += n * (n - 1); k >= 0; U -= n, k--) { + if (*(U + k) == 0.0) return -1; // The matrix U is singular + x[k] = B[k]; + for (i = k + 1; i < n; i++) x[k] -= x[i] * *(U + i); + x[k] /= *(U + k); + } + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Upper_Triangular_Inverse(double *U, int n) // +// // +// Description: // +// This routine calculates the inverse of the upper triangular matrix U. // +// The subdiagonal part of the matrix is not addressed. // +// The algorithm follows: // +// Let M be the inverse of U, then U M = I, // +// M[n-1][n-1] = 1.0 / U[n-1][n-1] and // +// M[i][j] = -( U[i][i+1] M[i+1][j] + ... + U[i][j] M[j][j] ) / U[i][i], // +// for i = n-2, ... , 0, j = n-1, ..., i+1. // +// // +// // +// Arguments: // +// double *U On input, the pointer to the first element of the matrix // +// whose upper triangular elements form the matrix which is // +// to be inverted. On output, the upper triangular part is // +// replaced by the inverse. The subdiagonal elements are // +// not modified. // +// int n The number of rows and/or columns of the matrix U. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix U is singular. // +// // +// Example: // +// #define N // +// double U[N][N]; // +// // +// (your code to create the matrix U) // +// err = Upper_Triangular_Inverse(&U[0][0], N); // +// if (err < 0) printf(" Matrix U is singular\n"); // +// else { // +// printf(" The inverse is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Upper_Triangular_Inverse(double *U, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + +// Invert the diagonal elements of the upper triangular matrix U. + + for (k = 0, p_k = U; k < n; p_k += (n + 1), k++) { + if (*p_k == 0.0) return -1; + else *p_k = 1.0 / *p_k; + } + +// Invert the remaining upper triangular matrix U. + + for (i = n - 2, p_i = U + n * (n - 2); i >=0; p_i -= n, i-- ) { + for (j = n - 1; j > i; j--) { + sum = 0.0; + for (k = i + 1, p_k = p_i + n; k <= j; p_k += n, k++ ) { + sum += *(p_i + k) * *(p_k + j); + } + *(p_i + j) = - *(p_i + i) * sum; + } + } + + return 0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: interchange_cols.c // +// Routine(s): // +// Interchange_Columns // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Interchange_Columns(double *A, int col1, int col2, int nrows, // +// int ncols) // +// // +// Description: // +// Interchange the columns 'col1' and 'col2' of the nrows x ncols // +// matrix A. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int col1 The column of A which is to be interchanged with col2. // +// int col2 The column of A which is to be interchanged with col1. // +// int nrows The number of rows matrix A. // +// int ncols The number of columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// double A[M][N]; // +// int i,j; // +// // +// (your code to initialize the matrix A, the column number i and column // +// number j) // +// // +// if ( (i >= 0) && ( i < N ) && ( j >= 0 ) && (j < N) ) // +// Interchange_Columns(&A[0][0], i, j, M, N); // +// printf("The matrix A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols) +{ + int i; + double *pA1, *pA2; + double temp; + + pA1 = A + col1; + pA2 = A + col2; + for (i = 0; i < nrows; pA1 += ncols, pA2 += ncols, i++) { + temp = *pA1; + *pA1 = *pA2; + *pA2 = temp; + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: interchange_rows.c // +// Routine(s): // +// Interchange_Rows // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Interchange_Rows(double *A, int row1, int row2, int ncols) // +// // +// Description: // +// Interchange the rows 'row1' and 'row2' of the nrows x ncols matrix A. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int row1 The row of A which is to be interchanged with row row2. // +// int row2 The row of A which is to be interchanged with row row1. // +// int ncols The number of columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// double A[M][N]; // +// int i, j; // +// // +// (your code to initialize the matrix A, the row number i and row number j) // +// // +// if ( (i >= 0) && ( i < M ) && (j > 0) && ( j < M ) ) // +// Interchange_Rows(&A[0][0], i, j, N); // +// printf("The matrix A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Interchange_Rows(double *A, int row1, int row2, int ncols) +{ + int i; + double *pA1, *pA2; + double temp; + + pA1 = A + row1 * ncols; + pA2 = A + row2 * ncols; + for (i = 0; i < ncols; i++) { + temp = *pA1; + *pA1++ = *pA2; + *pA2++ = temp; + } +} \ No newline at end of file diff --git a/calibration/mega_calibration.cpp b/calibration/mega_calibration.cpp new file mode 100644 index 0000000..5b6dcb2 --- /dev/null +++ b/calibration/mega_calibration.cpp @@ -0,0 +1,1911 @@ +/* +This code is a fusion of the modified version of the standard code from +the Adafruit LSM9DS1 library and the version of Magneto authored by S. James Remington +magneto 1.4 magnetometer/accelerometer calibration code +from http://sailboatinstruments.blogspot.com/2011/08/improved-magnetometer-calibration.html +tested and works with Code::Blocks 10.05 through 20.03 + +Example I2C hardware setup with the TinyS3: +LSM9DS1 -------- ESP32 +3V ------------- 3.3V +G -------------- GND or - +SCL ------------ 9 +SDA ------------ 8 +*/ + +#include +#include +#include +#include +#include + +// C-Libraries needed for Magneto +#include +#include +#include +#include +#include + +// I2C! +Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); + +#define LSM9DS1_SCK A5 +#define LSM9DS1_MISO 12 +#define LSM9DS1_MOSI A4 +#define LSM9DS1_XGCS 6 +#define LSM9DS1_MCS 5 + + + +#define MEASUREMENT_INTERVAL 100 // 100 ms between measurements +#define NUM_MEASUREMENTS 300 // +int numMeasurements = 0; +double sensorMagData[NUM_MEASUREMENTS][3]; // Static array to store the magnetometer measurements +double sensorAccData[NUM_MEASUREMENTS][3]; // Static array to store the accelerometer measurements + +void setupSensor() +{ + // IMPORTANT: Accuracy is inversely proportional to the sensor range! + // Ringinator works with + // 1.) Set the accelerometer range + lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ); + + // 2.) Set the magnetometer sensitivity + lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS); + + // 3.) Setup the gyroscope + lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS); +} + + +// Forward declarations of mymathlib.com routines +void Multiply_Self_Transpose(double*, double*, int, int); +void Get_Submatrix(double*, int, int, double*, int, int, int); +int Choleski_LU_Decomposition(double*, int); +int Choleski_LU_Inverse(double*, int); +void Multiply_Matrices(double*, double*, int, int, double*, int); +void Identity_Matrix(double*, int); + +int Hessenberg_Form_Elementary(double*, double*, int); +static void Hessenberg_Elementary_Transform(double*, double*, int[], int); + +void Copy_Vector(double*, double*, int); + +int QR_Hessenberg_Matrix(double*, double*, double[], double[], int, int); +static void One_Real_Eigenvalue(double[], double[], double[], int, double); +static void Two_Eigenvalues(double*, double*, double[], double[], int, int, double); +static void Update_Row(double*, double, double, int, int); +static void Update_Column(double*, double, double, int, int); +static void Update_Transformation(double*, double, double, int, int); +static void Double_QR_Iteration(double*, double*, int, int, int, double*, int); +static void Product_and_Sum_of_Shifts(double*, int, int, double*, double*, double*, int); +static int Two_Consecutive_Small_Subdiagonal(double*, int, int, int, double, double); +static void Double_QR_Step(double*, int, int, int, double, double, double*, int); +static void BackSubstitution(double*, double[], double[], int); +static void BackSubstitute_Real_Vector(double*, double[], double[], int, double, int); +static void BackSubstitute_Complex_Vector(double*, double[], double[], int, double, int); +static void Calculate_Eigenvectors(double*, double*, double[], double[], int); +static void Complex_Division(double, double, double, double, double*, double*); + +void Transpose_Square_Matrix(double*, int); + +void setup() +{ + Serial.begin(115200); + + while (!Serial) { + delay(1); // will pause Zero, Leonardo, etc until serial console opens + } + + Serial.println("LSM9DS1 data read demo"); + + // Try to initialise and warn if we couldn't detect the chip + if (!lsm.begin()) + { + Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!"); + while (1); + } + Serial.println("Found LSM9DS1 9DOF"); + + // helper to just set the default scaling we want, see above! + setupSensor(); + + delay(2000); // 2 Second delay because the TinyS3 has to be reset and the button press shakes the device + Serial.println("Collecting gyro data, hold still"); + // get gyro offset + float gxa = 0, gya = 0, gza = 0; //change to double if number is big + for (int i = 0; i < 300; i++) { + lsm.read(); /* ask it to read in the data */ + sensors_event_t a, m, g, temp; + lsm.getEvent(&a, &m, &g, &temp); + gxa += g.gyro.x; + gya += g.gyro.y; + gza += g.gyro.z; + } + Serial.println(F("gyro offsets")); + Serial.print(gxa / 300); + Serial.print(", "); + Serial.print(gya / 300); + Serial.print(", "); + Serial.println(gza / 300); + Serial.println(); + + Serial.println(F("rotate slowly and carefully in 3D")); + delay(3000); + Serial.println("starting"); + + + +} + +void loop() +{ + + if (numMeasurements < NUM_MEASUREMENTS) { + lsm.read(); /* ask it to read in the data */ + + /* Get a new sensor event */ + sensors_event_t a, m, g, temp; + + lsm.getEvent(&a, &m, &g, &temp); + sensorAccData[numMeasurements][0] = a.acceleration.x; + sensorAccData[numMeasurements][1] = a.acceleration.y; + sensorAccData[numMeasurements][2] = a.acceleration.z; + sensorMagData[numMeasurements][0] = m.magnetic.x; + sensorMagData[numMeasurements][1] = m.magnetic.y; + sensorMagData[numMeasurements][2] = m.magnetic.z; + numMeasurements++; + Serial.print("Accel X: "); Serial.print(a.acceleration.x); Serial.print(" m/s^2"); + Serial.print("\tY: "); Serial.print(a.acceleration.y); Serial.print(" m/s^2 "); + Serial.print("\tZ: "); Serial.print(a.acceleration.z); Serial.println(" m/s^2 "); + Serial.print("Mag X: "); Serial.print(m.magnetic.x); Serial.print(" uT"); + Serial.print("\tY: "); Serial.print(m.magnetic.y); Serial.print(" uT"); + Serial.print("\tZ: "); Serial.print(m.magnetic.z); Serial.println(" uT"); + } else { + Serial.println("Measurements done. Please wait for the calibrator to setup!"); + // MKWii Online music playing in the background... + delay(2000); + for (int sensor=0; sensor<2; sensor++) { + if (sensor==0) { + Serial.println("Calibration parameters of the magnetometer: "); + } else { + Serial.println("Calibration parameters of the accelerometer: "); + } + double *D, *S, *C, *S11, *S12, *S12t, *S22, *S22_1, *S22a, *S22b, *SS, *E, *d, *U, *SSS; + double *eigen_real, *eigen_imag, *v1, *v2, *v, *Q, *Q_1, *B, *QB, J, hmb, *SSSS; + int *p; + int i, j, index; + int ngood = NUM_MEASUREMENTS; + int nlines = 0; + double maxval, norm, btqb, *eigen_real3, *eigen_imag3, *Dz, *vdz, *SQ, *A_1, hm, norm1, norm2, norm3; + double x, y, z, x2, nxsrej, xs, xave; + double *raw; //raw obs + + // calculate mean (norm) and standard deviation for possible outlier rejection + + xs=0; + + xave=0; + + for(i = 0; i < NUM_MEASUREMENTS; i++) { + if (sensor==0) { + x = sensorMagData[i][0]; + y = sensorMagData[i][1]; + z = sensorMagData[i][2]; + } else { + x = sensorAccData[i][0]; + y = sensorAccData[i][1]; + z = sensorAccData[i][2]; + } + x2 = x*x + y*y + z*z; + xs += x2; + xave += sqrt(x2); + } + xave = xave/NUM_MEASUREMENTS; //mean vector length + xs = sqrt(xs/NUM_MEASUREMENTS -(xave*xave)); //std. dev. + + // summarize statistics, give user opportunity to reject outlying measurements + + Serial.print("\nAverage magnitude (default Hm) and sigma of 300 vectors = "); + Serial.print(xave, 1); + Serial.println(xs, 1); + + nxsrej = 0; // You can set the rejection criteria here, but the LSM9DS1 is pretty solid, so 0 is a good choice + if (nxsrej > 0) { + Serial.print("Rejecting measurements if abs(vector_length-average)/(std. dev.) > "); + Serial.println(nxsrej, 1); + + // outlier rejection, count remaining lines + + for(i = 0; i < NUM_MEASUREMENTS; i++) { + if (sensor==0) { + x = sensorMagData[i][0]; + y = sensorMagData[i][1]; + z = sensorMagData[i][2]; + } else { + x = sensorAccData[i][0]; + y = sensorAccData[i][1]; + z = sensorAccData[i][2]; + } + x2 = sqrt(x*x + y*y + z*z); //vector length + x2 = fabs(x2 - xave)/xs; //standard deviations from mean + if (x2 > nxsrej) { + ngood--; + Serial.print("We reject: "); + Serial.print(x,2); + Serial.print(y,2); + Serial.println(z,2); + } + } + Serial.print("We reject: "); + Serial.print(NUM_MEASUREMENTS-ngood); + Serial.println(" measurements!"); + } + + // allocate array space for accepted measurements + D = (double*)malloc(10 * ngood * sizeof(double)); + raw = (double*)malloc(3 * ngood * sizeof(double)); + + j = 0; //array index for good measurements + + for(i = 0; i < NUM_MEASUREMENTS; i++) { + if (sensor==0) { + x = sensorMagData[i][0]; + y = sensorMagData[i][1]; + z = sensorMagData[i][2]; + } else { + x = sensorAccData[i][0]; + y = sensorAccData[i][1]; + z = sensorAccData[i][2]; + } + x2 = sqrt(x*x + y*y + z*z); //vector length + x2 = fabs(x2 - xave)/xs; //standard deviation from mean + if ((nxsrej == 0) || (x2 <= nxsrej)) { + + raw[3*j] = x; + raw[3*j+1] = y; + raw[3*j+2] = z; + D[j] = x * x; + D[ngood+j] = y * y; + D[ngood*2+j] = z * z; + D[ngood*3+j] = 2.0 * y * z; + D[ngood*4+j] = 2.0 * x * z; + D[ngood*5+j] = 2.0 * x * y; + D[ngood*6+j] = 2.0 * x; + D[ngood*7+j] = 2.0 * y; + D[ngood*8+j] = 2.0 * z; + D[ngood*9+j] = 1.0; + j++; //count good measurements + } + } + + if (j != ngood) { + Serial.println("Internal indexing error!"); + while(1); //HALT + } + + nlines = ngood; //number to process + + hm=xave; // CHANGE IF YOU DISAGREE + Serial.print("Hm is set to: "); + Serial.println(hm,2); + + // allocate memory for matrix S + S = (double*)malloc(10 * 10 * sizeof(double)); + Multiply_Self_Transpose(S, D, 10, nlines); + + // Create pre-inverted constraint matrix C + C = (double*)malloc(6 * 6 * sizeof(double)); + C[0] = 0.0; C[1] = 0.5; C[2] = 0.5; C[3] = 0.0; C[4] = 0.0; C[5] = 0.0; + C[6] = 0.5; C[7] = 0.0; C[8] = 0.5; C[9] = 0.0; C[10] = 0.0; C[11] = 0.0; + C[12] = 0.5; C[13] = 0.5; C[14] = 0.0; C[15] = 0.0; C[16] = 0.0; C[17] = 0.0; + C[18] = 0.0; C[19] = 0.0; C[20] = 0.0; C[21] = -0.25; C[22] = 0.0; C[23] = 0.0; + C[24] = 0.0; C[25] = 0.0; C[26] = 0.0; C[27] = 0.0; C[28] = -0.25; C[29] = 0.0; + C[30] = 0.0; C[31] = 0.0; C[32] = 0.0; C[33] = 0.0; C[34] = 0.0; C[35] = -0.25; + + S11 = (double*)malloc(6 * 6 * sizeof(double)); + Get_Submatrix(S11, 6, 6, S, 10, 0, 0); + S12 = (double*)malloc(6 * 4 * sizeof(double)); + Get_Submatrix(S12, 6, 4, S, 10, 0, 6); + S12t = (double*)malloc(4 * 6 * sizeof(double)); + Get_Submatrix(S12t, 4, 6, S, 10, 6, 0); + S22 = (double*)malloc(4 * 4 * sizeof(double)); + Get_Submatrix(S22, 4, 4, S, 10, 6, 6); + + S22_1 = (double*)malloc(4 * 4 * sizeof(double)); + for(i = 0; i < 16; i++) + S22_1[i] = S22[i]; + Choleski_LU_Decomposition(S22_1, 4); + Choleski_LU_Inverse(S22_1, 4); + + // Calculate S22a = S22_1 * S12t 4*6 = 4x4 * 4x6 C = AB + S22a = (double*)malloc(4 * 6 * sizeof(double)); + Multiply_Matrices(S22a, S22_1, 4, 4, S12t, 6); + + // Then calculate S22b = S12 * S22a ( 6x6 = 6x4 * 4x6) + S22b = (double*)malloc(6 * 6 * sizeof(double)); + Multiply_Matrices(S22b, S12, 6, 4, S22a, 6); + + // Calculate SS = S11 - S22b + SS = (double*)malloc(6 * 6 * sizeof(double)); + for(i = 0; i < 36; i++) + SS[i] = S11[i] - S22b[i]; + E = (double*)malloc(6 * 6 * sizeof(double)); + Multiply_Matrices(E, C, 6, 6, SS, 6); + + SSS = (double*)malloc(6 * 6 * sizeof(double)); + Hessenberg_Form_Elementary(E, SSS, 6); + + eigen_real = (double*)malloc(6 * sizeof(double)); + eigen_imag = (double*)malloc(6 * sizeof(double)); + + QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100); + + index = 0; + maxval = eigen_real[0]; + for(i = 1; i < 6; i++) + { + if(eigen_real[i] > maxval) + { + maxval = eigen_real[i]; + index = i; + } + } + + v1 = (double*)malloc(6 * sizeof(double)); + + v1[0] = SSS[index]; + v1[1] = SSS[index+6]; + v1[2] = SSS[index+12]; + v1[3] = SSS[index+18]; + v1[4] = SSS[index+24]; + v1[5] = SSS[index+30]; + + // normalize v1 + norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]); + v1[0] /= norm; + v1[1] /= norm; + v1[2] /= norm; + v1[3] /= norm; + v1[4] /= norm; + v1[5] /= norm; + + if(v1[0] < 0.0) + { + v1[0] = -v1[0]; + v1[1] = -v1[1]; + v1[2] = -v1[2]; + v1[3] = -v1[3]; + v1[4] = -v1[4]; + v1[5] = -v1[5]; + } + + // Calculate v2 = S22a * v1 ( 4x1 = 4x6 * 6x1) + v2 = (double*)malloc(4 * sizeof(double)); + Multiply_Matrices(v2, S22a, 4, 6, v1, 1); + + v = (double*)malloc(10 * sizeof(double)); + + v[0] = v1[0]; + v[1] = v1[1]; + v[2] = v1[2]; + v[3] = v1[3]; + v[4] = v1[4]; + v[5] = v1[5]; + v[6] = -v2[0]; + v[7] = -v2[1]; + v[8] = -v2[2]; + v[9] = -v2[3]; + + Q = (double*)malloc(3 * 3 * sizeof(double)); + + Q[0] = v[0]; + Q[1] = v[5]; + Q[2] = v[4]; + Q[3] = v[5]; + Q[4] = v[1]; + Q[5] = v[3]; + Q[6] = v[4]; + Q[7] = v[3]; + Q[8] = v[2]; + + U = (double*)malloc(3 * sizeof(double)); + + U[0] = v[6]; + U[1] = v[7]; + U[2] = v[8]; + Q_1 = (double*)malloc(3 * 3 * sizeof(double)); + for(i = 0; i < 9; i++) + Q_1[i] = Q[i]; + Choleski_LU_Decomposition(Q_1, 3); + Choleski_LU_Inverse(Q_1, 3); + + // Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1) + B = (double*)malloc(3 * sizeof(double)); + Multiply_Matrices(B, Q_1, 3, 3, U, 1); + B[0] = -B[0]; // x-axis combined bias + B[1] = -B[1]; // y-axis combined bias + B[2] = -B[2]; // z-axis combined bias + + // for(i = 0; i < 3; i++) + printf("\r\nCombined bias vector B:\r\n"); + printf("%8.2lf %8.2lf %8.2lf \r\n", B[0],B[1],B[2]); + + // First calculate QB = Q * B ( 3x1 = 3x3 * 3x1) + QB = (double*)malloc(3 * sizeof(double)); + Multiply_Matrices(QB, Q, 3, 3, B, 1); + + // Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1) + Multiply_Matrices(&btqb, B, 1, 3, QB, 1); + + // Calculate hmb = sqrt(btqb - J). + J = v[9]; + hmb = sqrt(btqb - J); + + // Calculate SQ, the square root of matrix Q + SSSS = (double*)malloc(3 * 3 * sizeof(double)); + Hessenberg_Form_Elementary(Q, SSSS, 3); + + eigen_real3 = (double*)malloc(3 * sizeof(double)); + eigen_imag3 = (double*)malloc(3 * sizeof(double)); + QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100); + + // normalize eigenvectors + norm1 = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]); + SSSS[0] /= norm1; + SSSS[3] /= norm1; + SSSS[6] /= norm1; + norm2 = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]); + SSSS[1] /= norm2; + SSSS[4] /= norm2; + SSSS[7] /= norm2; + norm3 = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]); + SSSS[2] /= norm3; + SSSS[5] /= norm3; + SSSS[8] /= norm3; + + Dz = (double*)malloc(3 * 3 * sizeof(double)); + for(i = 0; i < 9; i++) + Dz[i] = 0.0; + Dz[0] = sqrt(eigen_real3[0]); + Dz[4] = sqrt(eigen_real3[1]); + Dz[8] = sqrt(eigen_real3[2]); + + vdz = (double*)malloc(3 * 3 * sizeof(double)); + Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3); + + Transpose_Square_Matrix(SSSS, 3); + + SQ = (double*)malloc(3 * 3 * sizeof(double)); + Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3); + + // hm = 0.569; + A_1 = (double*)malloc(3 * 3 * sizeof(double)); + + for(i = 0; i < 9; i++) + A_1[i] = SQ[i] * hm / hmb; + + + + + Serial.print("\nCorrection matrix Ainv, using Hm="); + Serial.print(hm, 1); // Print 'hm' with 1 decimal place + Serial.println(":"); + + for (int i = 0; i < 3; i++) { + Serial.print(A_1[i * 3], 5); // Print A_1 values with 5 decimal places + Serial.print(" "); + Serial.print(A_1[i * 3 + 1], 5); + Serial.print(" "); + Serial.println(A_1[i * 3 + 2], 5); + } + + Serial.println("\nWhere Hcalc = Ainv*(H-B)\n"); + + Serial.println("Code initialization statements..."); + Serial.print("\nfloat B[3] = {"); + Serial.print(B[0], 2); // Print B values with 2 decimal places + Serial.print(","); + Serial.print(B[1], 2); + Serial.print(","); + Serial.print(B[2], 2); + Serial.println("};"); + + Serial.println("\nfloat Ainv[3][3] = {"); + Serial.print(" {"); + Serial.print(A_1[0], 5); + Serial.print(","); + Serial.print(A_1[1], 5); + Serial.print(","); + Serial.print(A_1[2], 5); + Serial.println("},"); + Serial.print(" {"); + Serial.print(A_1[3], 5); + Serial.print(","); + Serial.print(A_1[4], 5); + Serial.print(","); + Serial.print(A_1[5], 5); + Serial.println("},"); + Serial.print(" {"); + Serial.print(A_1[6], 5); + Serial.print(","); + Serial.print(A_1[7], 5); + Serial.print(","); + Serial.print(A_1[8], 5); + Serial.println("}"); + Serial.println("};"); + + Serial.print("\nRMS corrected vector length "); + Serial.println(sqrt(xs / nlines), 6); // Print RMS value with 6 decimal places + + Serial.print("\nRMS residual "); + Serial.println(1.0 - sqrt(xs / nlines) / hm, 6); // Print RMS residual with 6 decimal places + + + // printf("\r\nCorrection matrix Ainv, using Hm=%8.1f:\r\n",hm); + // for(i = 0; i < 3; i++) + // printf("%9.5lf %9.5lf %9.5lf\r\n", A_1[i*3], A_1[i*3+1], A_1[i*3+2]); + // printf("\r\nWhere Hcalc = Ainv*(H-B)\r\n"); + + // printf("\r\nCode initialization statements...\r\n"); + // printf("\r\n float B[3]\r\n {%8.2lf,%8.2lf,%8.2lf};\r\n",B[0],B[1],B[2]); + // printf("\r\n float Ainv[3][3]\r\n {{%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[0],A_1[1],A_1[2]); + // printf(" {%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[3],A_1[4],A_1[5]); + // printf(" {%9.5lf,%9.5lf,%9.5lf}};\r\n",A_1[6],A_1[7],A_1[8]); + + // printf("\r\nRMS corrected vector length %9.6f\n",sqrt(xs/nlines)); + // printf("\r\nRMS residual %9.6f\n",1.0-sqrt(xs/nlines)/hm); + + + // dump corrected measurements if desired; I'm not removing the possibility of doing this + + // printf(" output filename for corrected values (csv) "); + // scanf("%s",&filename); + // if(strlen(filename) > 1) { + + // fp2 = fopen(filename, "w"); + // float xc,yc,zc; + // xs=0; + // for(i = 0; i // required for memcpy() + +void Get_Submatrix(double *S, int mrows, int mcols, + double *A, int ncols, int row, int col) +{ + int number_of_bytes = sizeof(double) * mcols; + + for (A += row * ncols + col; mrows > 0; A += ncols, S+= mcols, mrows--) + memcpy(S, A, number_of_bytes); +} + +int Lower_Triangular_Solve(double *L, double B[], double x[], int n); +int Lower_Triangular_Inverse(double *L, int n); +int Upper_Triangular_Solve(double *U, double B[], double x[], int n); + +int Choleski_LU_Decomposition(double *A, int n) +{ + int i, k, p; + double *p_Lk0; // pointer to L[k][0] + double *p_Lkp; // pointer to L[k][p] + double *p_Lkk; // pointer to diagonal element on row k. + double *p_Li0; // pointer to L[i][0] + double reciprocal; + + for (k = 0, p_Lk0 = A; k < n; p_Lk0 += n, k++) { + +// Update pointer to row k diagonal element. + + p_Lkk = p_Lk0 + k; + +// Calculate the difference of the diagonal element in row k +// from the sum of squares of elements row k from column 0 to +// column k-1. + + for (p = 0, p_Lkp = p_Lk0; p < k; p_Lkp += 1, p++) + *p_Lkk -= *p_Lkp * *p_Lkp; + +// If diagonal element is not positive, return the error code, +// the matrix is not positive definite symmetric. + + if ( *p_Lkk <= 0.0 ) return -1; + +// Otherwise take the square root of the diagonal element. + + *p_Lkk = sqrt( *p_Lkk ); + reciprocal = 1.0 / *p_Lkk; + +// For rows i = k+1 to n-1, column k, calculate the difference +// between the i,k th element and the inner product of the first +// k-1 columns of row i and row k, then divide the difference by +// the diagonal element in row k. +// Store the transposed element in the upper triangular matrix. + + p_Li0 = p_Lk0 + n; + for (i = k + 1; i < n; p_Li0 += n, i++) { + for (p = 0; p < k; p++) + *(p_Li0 + k) -= *(p_Li0 + p) * *(p_Lk0 + p); + *(p_Li0 + k) *= reciprocal; + *(p_Lk0 + i) = *(p_Li0 + k); + } + } + return 0; +} + +int Choleski_LU_Solve(double *LU, double B[], double x[], int n) +{ + +// Solve the linear equation Ly = B for y, where L is a lower +// triangular matrix. + + if ( Lower_Triangular_Solve(LU, B, x, n) < 0 ) return -1; + +// Solve the linear equation Ux = y, where y is the solution +// obtained above of Ly = B and U is an upper triangular matrix. + + return Upper_Triangular_Solve(LU, x, x, n); +} + +int Choleski_LU_Inverse(double *LU, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + + if ( Lower_Triangular_Inverse(LU, n) < 0 ) return -1; + +// Premultiply L inverse by the transpose of L inverse. + + for (i = 0, p_i = LU; i < n; i++, p_i += n) { + for (j = 0, p_j = LU; j <= i; j++, p_j += n) { + sum = 0.0; + for (k = i, p_k = p_i; k < n; k++, p_k += n) + sum += *(p_k + i) * *(p_k + j); + *(p_i + j) = sum; + *(p_j + i) = sum; + } + } + + return 0; +} + +void Multiply_Matrices(double *C, double *A, int nrows, int ncols, + double *B, int mcols) +{ + double *pB; + double *p_B; + int i,j,k; + + for (i = 0; i < nrows; A += ncols, i++) + for (p_B = B, j = 0; j < mcols; C++, p_B++, j++) { + pB = p_B; + *C = 0.0; + for (k = 0; k < ncols; pB += mcols, k++) + *C += *(A+k) * *pB; + } +} + +void Identity_Matrix(double *A, int n) +{ + int i,j; + + for (i = 0; i < n - 1; i++) { + *A++ = 1.0; + for (j = 0; j < n; j++) *A++ = 0.0; + } + *A = 1.0; +} + +void Interchange_Rows(double *A, int row1, int row2, int ncols); +void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols); +void Identity_Matrix(double *A, int n); +void Copy_Vector(double *d, double *s, int n); + +// Internally Defined Routines +static void Hessenberg_Elementary_Transform(double* H, double *S, int perm[], int n); + +int Hessenberg_Form_Elementary(double *A, double* S, int n) +{ + int i, j, k, col, row; + int* perm; + double *p_row, *pS_row; + double max; + double s; + double *pA, *pB, *pC, *pS; + + // n x n matrices for which n <= 2 are already in Hessenberg form + + if (n <= 1) { *S = 1.0; return 0; } + if (n == 2) { *S++ = 1.0; *S++ = 0.0; *S++ = 1.0; *S = 0.0; return 0; } + + // Allocate working memory + + perm = (int*) malloc(n * sizeof(int)); + if (perm == NULL) return -1; // not enough memory + + // For each column use Elementary transformations + // to zero the entries below the subdiagonal. + + p_row = A + n; + pS_row = S + n; + for (col = 0; col < (n - 2); p_row += n, pS_row += n, col++) { + + // Find the row in column "col" with maximum magnitude where + // row >= col + 1. + + row = col + 1; + perm[row] = row; + for (pA = p_row + col, max = 0.0, i = row; i < n; pA += n, i++) + if (fabs(*pA) > max) { perm[row] = i; max = fabs(*pA); } + + // If perm[row] != row, then interchange row "row" and row + // perm[row] and interchange column "row" and column perm[row]. + + if ( perm[row] != row ) { + Interchange_Rows(A, row, perm[row], n); + Interchange_Columns(A, row, perm[row], n, n); + } + + // Zero out the components lying below the subdiagonal. + + pA = p_row + n; + pS = pS_row + n; + for (i = col + 2; i < n; pA += n, pS += n, i++) { + s = *(pA + col) / *(p_row + col); + for (j = 0; j < n; j++) + *(pA + j) -= *(p_row + j) * s; + *(pS + col) = s; + for (j = 0, pB = A + col + 1, pC = A + i; j < n; pB +=n, pC += n, j++) + *pB += s * *pC; + } + } + pA = A + n + n; + pS = S + n + n; + for (i = 2; i < n; pA += n, pS += n, i++) Copy_Vector(pA, pS, i - 1); + + Hessenberg_Elementary_Transform(A, S, perm, n); + + free(perm); + return 0; +} + + +static void Hessenberg_Elementary_Transform(double *H, double* S, int perm[], + int n) +{ + int i, j; + double *pS, *pH; + double x; + + Identity_Matrix(S, n); + for (i = n - 2; i >= 1; i--) { + pH = H + n * (i + 1); + pS = S + n * (i + 1); + for (j = i + 1; j < n; pH += n, pS += n, j++) { + *(pS + i) = *(pH + i - 1); + *(pH + i - 1) = 0.0; + } + if (perm[i] != i) { + pS = S + n * i; + pH = S + n * perm[i]; + for (j = i; j < n; j++) { + *(pS + j) = *(pH + j); + *(pH + j) = 0.0; + } + *(pH + i) = 1.0; + } + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: qr_hessenberg_matrix.c // +// Routine(s): // +// QR_Hessenberg_Matrix // +//////////////////////////////////////////////////////////////////////////////// + +#include // required for fabs() and sqrt() +#include // required for DBL_EPSILON + + +// Internally Defined Routines +static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], + double eigen_imag[], int row, double shift); +static void Two_Eigenvalues(double *H, double *S, double eigen_real[], + double eigen_imag[], int n, int k, double t); +static void Update_Row(double *Hrow, double cos, double sin, int n, int k); +static void Update_Column(double* H, double cos, double sin, int n, int k); +static void Update_Transformation(double *S, double cos, double sin, + int n, int k); +static void Double_QR_Iteration(double *H, double *S, int row, int min_row, + int n, double* shift, int iteration); +static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, + double* shift, double *trace, double *det, int iteration); +static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, + int max_row, int n, double trace, double det); +static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, + double trace, double det, double *S, int n); +static void Complex_Division(double x, double y, double u, double v, + double* a, double* b); +static void BackSubstitution(double *H, double eigen_real[], + double eigen_imag[], int n); +static void BackSubstitute_Real_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n); +static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n); +static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], + double eigen_imag[], int n); + +int QR_Hessenberg_Matrix( double *H, double *S, double eigen_real[], + double eigen_imag[], int n, int max_iteration_count) +{ + int i; + int row; + int iteration; + int found_eigenvalue; + double shift = 0.0; + double* pH; + + for ( row = n - 1; row >= 0; row--) { + found_eigenvalue = 0; + for (iteration = 1; iteration <= max_iteration_count; iteration++) { + + // Search for small subdiagonal element + + for (i = row, pH = H + row * n; i > 0; i--, pH -= n) + if (fabs(*(pH + i - 1 )) <= DBL_EPSILON * + ( fabs(*(pH - n + i - 1)) + fabs(*(pH + i)) ) ) break; + + // If the subdiagonal element on row "row" is small, then + // that row element is an eigenvalue. If the subdiagonal + // element on row "row-1" is small, then the eigenvalues + // of the 2x2 diagonal block consisting rows "row-1" and + // "row" are eigenvalues. Otherwise perform a double QR + // iteration. + + switch(row - i) { + case 0: // One real eigenvalue + One_Real_Eigenvalue(pH, eigen_real, eigen_imag, i, shift); + found_eigenvalue = 1; + break; + case 1: // Either two real eigenvalues or a complex pair + row--; + Two_Eigenvalues(H, S, eigen_real, eigen_imag, n, row, shift); + found_eigenvalue = 1; + break; + default: + Double_QR_Iteration(H, S, i, row, n, &shift, iteration); + } + if (found_eigenvalue) break; + } + if (iteration > max_iteration_count) return -1; + } + + BackSubstitution(H, eigen_real, eigen_imag, n); + Calculate_Eigenvectors(H, S, eigen_real, eigen_imag, n); + + return 0; +} +static void One_Real_Eigenvalue(double Hrow[], double eigen_real[], + double eigen_imag[], int row, double shift) +{ + Hrow[row] += shift; + eigen_real[row] = Hrow[row]; + eigen_imag[row] = 0.0; +} +static void Two_Eigenvalues(double *H, double* S, double eigen_real[], + double eigen_imag[], int n, int row, double shift) +{ + double p, q, x, discriminant, r; + double cos, sin; + double *Hrow = H + n * row; + double *Hnextrow = Hrow + n; + int nextrow = row + 1; + + p = 0.5 * (Hrow[row] - Hnextrow[nextrow]); + x = Hrow[nextrow] * Hnextrow[row]; + discriminant = p * p + x; + Hrow[row] += shift; + Hnextrow[nextrow] += shift; + if (discriminant > 0.0) { // pair of real roots + q = sqrt(discriminant); + if (p < 0.0) q = p - q; else q += p; + eigen_real[row] = Hnextrow[nextrow] + q; + eigen_real[nextrow] = Hnextrow[nextrow] - x / q; + eigen_imag[row] = 0.0; + eigen_imag[nextrow] = 0.0; + r = sqrt(Hnextrow[row]*Hnextrow[row] + q * q); + sin = Hnextrow[row] / r; + cos = q / r; + Update_Row(Hrow, cos, sin, n, row); + Update_Column(H, cos, sin, n, row); + Update_Transformation(S, cos, sin, n, row); + } + else { // pair of complex roots + eigen_real[nextrow] = eigen_real[row] = Hnextrow[nextrow] + p; + eigen_imag[row] = sqrt(fabs(discriminant)); + eigen_imag[nextrow] = -eigen_imag[row]; + } +} +static void Update_Row(double *Hrow, double cos, double sin, int n, int row) +{ + double x; + double *Hnextrow = Hrow + n; + int i; + + for (i = row; i < n; i++) { + x = Hrow[i]; + Hrow[i] = cos * x + sin * Hnextrow[i]; + Hnextrow[i] = cos * Hnextrow[i] - sin * x; + } +} +static void Update_Column(double* H, double cos, double sin, int n, int col) +{ + double x; + int i; + int next_col = col + 1; + + for (i = 0; i <= next_col; i++, H += n) { + x = H[col]; + H[col] = cos * x + sin * H[next_col]; + H[next_col] = cos * H[next_col] - sin * x; + } +} +static void Update_Transformation(double *S, double cos, double sin, + int n, int k) +{ + double x; + int i; + int k1 = k + 1; + + for (i = 0; i < n; i++, S += n) { + x = S[k]; + S[k] = cos * x + sin * S[k1]; + S[k1] = cos * S[k1] - sin * x; + } +} +static void Double_QR_Iteration(double *H, double *S, int min_row, int max_row, + int n, double* shift, int iteration) +{ + int k; + double trace, det; + + Product_and_Sum_of_Shifts(H, n, max_row, shift, &trace, &det, iteration); + k = Two_Consecutive_Small_Subdiagonal(H, min_row, max_row, n, trace, det); + Double_QR_Step(H, min_row, max_row, k, trace, det, S, n); +} +static void Product_and_Sum_of_Shifts(double *H, int n, int max_row, + double* shift, double *trace, double *det, int iteration) +{ + double *pH = H + max_row * n; + double *p_aux; + int i; + int min_col = max_row - 1; + + if ( ( iteration % 10 ) == 0 ) { + *shift += pH[max_row]; + for (i = 0, p_aux = H; i <= max_row; p_aux += n, i++) + p_aux[i] -= pH[max_row]; + p_aux = pH - n; + *trace = fabs(pH[min_col]) + fabs(p_aux[min_col - 1]); + *det = *trace * *trace; + *trace *= 1.5; + } + else { + p_aux = pH - n; + *trace = p_aux[min_col] + pH[max_row]; + *det = p_aux[min_col] * pH[max_row] - p_aux[max_row] * pH[min_col]; + } +}; + + +//////////////////////////////////////////////////////////////////////////////// +// static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, // +// int max_row, int n, double trace, double det) // +// // +// Description: // +// To reduce the amount of computation in Francis' double QR step search // +// for two consecutive small subdiagonal elements from row nn to row m, // +// where m < nn. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// int min_row // +// The row in which to end the search (search is from upwards). // +// int max_row // +// The row in which to begin the search. // +// int n // +// The dimension of H. // +// double trace // +// The trace of the lower 2 x 2 block diagonal matrix. // +// double det // +// The determinant of the lower 2 x 2 block diagonal matrix. // +// // +// Return Value: // +// Row with negligible subdiagonal element or min_row if none found. // +//////////////////////////////////////////////////////////////////////////////// +// // +static int Two_Consecutive_Small_Subdiagonal(double* H, int min_row, + int max_row, int n, double trace, double det) +{ + double x, y ,z, s; + double* pH; + int i, k; + + for (k = max_row - 2, pH = H + k * n; k >= min_row; pH -= n, k--) { + x = (pH[k] * ( pH[k] - trace ) + det) / pH[n+k] + pH[k+1]; + y = pH[k] + pH[n+k+1] - trace; + z = pH[n + n + k + 1]; + s = fabs(x) + fabs(y) + fabs(z); + x /= s; + y /= s; + z /= s; + if (k == min_row) break; + if ( (fabs(pH[k-1]) * (fabs(y) + fabs(z)) ) <= + DBL_EPSILON * fabs(x) * + (fabs(pH[k-1-n]) + fabs(pH[k]) + fabs(pH[n + k + 1])) ) break; + } + for (i = k+2, pH = H + i * n; i <= max_row; pH += n, i++) pH[i-2] = 0.0; + for (i = k+3, pH = H + i * n; i <= max_row; pH += n, i++) pH[i-3] = 0.0; + return k; +}; + + +//////////////////////////////////////////////////////////////////////////////// +// static void Double_QR_Step(double *H, int min_row, int max_row, // +// int min_col, double *S, int n) // +// // +// Description: // +// Perform Francis' double QR step from rows 'min_row' to 'max_row' // +// and columns 'min_col' to 'max_row'. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// int min_row // +// The row in which to begin. // +// int max_row // +// The row in which to end. // +// int min_col // +// The column in which to begin. // +// double trace // +// The trace of the lower 2 x 2 block diagonal matrix. // +// double det // +// The determinant of the lower 2 x 2 block diagonal matrix. // +// double *S // +// Pointer to the first element of the transformation matrix. // +// int n // +// The dimensions of H and S. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Double_QR_Step(double *H, int min_row, int max_row, int min_col, + double trace, double det, double *S, int n) +{ + double s, x, y, z; + double a, b, c; + double *pH; + double *tH; + double *pS; + int i,j,k; + int last_test_row_col = max_row - 1; + + k = min_col; + pH = H + min_col * n; + a = (pH[k] * ( pH[k] - trace ) + det) / pH[n+k] + pH[k+1]; + b = pH[k] + pH[n+k+1] - trace; + c = pH[n + n + k + 1]; + s = fabs(a) + fabs(b) + fabs(c); + a /= s; + b /= s; + c /= s; + + for (; k <= last_test_row_col; k++, pH += n) { + if ( k > min_col ) { + c = (k == last_test_row_col) ? 0.0 : pH[n + n + k - 1]; + x = fabs(pH[k-1]) + fabs(pH[n + k - 1]) + fabs(c); + if ( x == 0.0 ) continue; + a = pH[k - 1] / x; + b = pH[n + k - 1] / x; + c /= x; + } + s = sqrt( a * a + b * b + c * c ); + if (a < 0.0) s = -s; + if ( k > min_col ) pH[k-1] = -s * x; + else if (min_row != min_col) pH[k-1] = -pH[k-1]; + a += s; + x = a / s; + y = b / s; + z = c / s; + b /= a; + c /= a; + + // Update rows k, k+1, k+2 + for (j = k; j < n; j++) { + a = pH[j] + b * pH[n+j]; + if ( k != last_test_row_col ) { + a += c * pH[n + n + j]; + pH[n + n + j] -= a * z; + } + pH[n + j] -= a * y; + pH[j] -= a * x; + } + + // Update column k+1 + + j = k + 3; + if (j > max_row) j = max_row; + for (i = 0, tH = H; i <= j; i++, tH += n) { + a = x * tH[k] + y * tH[k+1]; + if ( k != last_test_row_col ) { + a += z * tH[k+2]; + tH[k+2] -= a * c; + } + tH[k+1] -= a * b; + tH[k] -= a; + } + + // Update transformation matrix + + for (i = 0, pS = S; i < n; pS += n, i++) { + a = x * pS[k] + y * pS[k+1]; + if ( k != last_test_row_col ) { + a += z * pS[k+2]; + pS[k+2] -= a * c; + } + pS[k+1] -= a * b; + pS[k] -= a; + } + }; +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitution(double *H, double eigen_real[], // +// double eigen_imag[], int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitution(double *H, double eigen_real[], + double eigen_imag[], int n) +{ + double zero_tolerance; + double *pH; + int i, j, row; + + // Calculate the zero tolerance + + pH = H; + zero_tolerance = fabs(pH[0]); + for (pH += n, i = 1; i < n; pH += n, i++) + for (j = i-1; j < n; j++) zero_tolerance += fabs(pH[j]); + zero_tolerance *= DBL_EPSILON; + + // Start Backsubstitution + + for (row = n-1; row >= 0; row--) { + if (eigen_imag[row] == 0.0) + BackSubstitute_Real_Vector(H, eigen_real, eigen_imag, row, + zero_tolerance, n); + else if ( eigen_imag[row] < 0.0 ) + BackSubstitute_Complex_Vector(H, eigen_real, eigen_imag, row, + zero_tolerance, n); + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitute_Real_Vector(double *H, double eigen_real[], // +// double eigen_imag[], int row, double zero_tolerance, int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int row // +// double zero_tolerance // +// Zero substitute. To avoid dividing by zero. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitute_Real_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n) +{ + double *pH; + double *pV; + double x,y; + double u[4]; + double v[2]; + int i,j,k; + + k = row; + pH = H + row * n; + pH[row] = 1.0; + for (i = row - 1, pH -= n; i >= 0; i--, pH -= n) { + u[0] = pH[i] - eigen_real[row]; + v[0] = pH[row]; + pV = H + n * k; + for (j = k; j < row; j++, pV += n) v[0] += pH[j] * pV[row]; + if ( eigen_imag[i] < 0.0 ) { + u[3] = u[0]; + v[1] = v[0]; + } else { + k = i; + if (eigen_imag[i] == 0.0) { + if (u[0] != 0.0) pH[row] = - v[0] / u[0]; + else pH[row] = - v[0] / zero_tolerance; + } else { + u[1] = pH[i+1]; + u[2] = pH[n+i]; + x = (eigen_real[i] - eigen_real[row]); + x *= x; + x += eigen_imag[i] * eigen_imag[i]; + pH[row] = (u[1] * v[1] - u[3] * v[0]) / x; + if ( fabs(u[1]) > fabs(u[3]) ) + pH[n+row] = -(v[0] + u[0] * pH[row]) / u[1]; + else + pH[n+row] = -(v[1] + u[2] * pH[row]) / u[3]; + } + } + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], // +// double eigen_imag[], int row, double zero_tolerance, int n) // +// // +// Description: // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int row // +// double zero_tolerance // +// Zero substitute. To avoid dividing by zero. // +// int n // +// The dimension of H, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void BackSubstitute_Complex_Vector(double *H, double eigen_real[], + double eigen_imag[], int row, double zero_tolerance, int n) +{ + double *pH; + double *pV; + double x,y; + double u[4]; + double v[2]; + double w[2]; + int i,j,k; + + k = row - 1; + pH = H + n * row; + if ( fabs(pH[k]) > fabs(pH[row-n]) ) { + pH[k-n] = - (pH[row] - eigen_real[row]) / pH[k]; + pH[row-n] = -eigen_imag[row] / pH[k]; + } + else + Complex_Division(-pH[row-n], 0.0, + pH[k-n]-eigen_real[row], eigen_imag[row], &pH[k-n], &pH[row-n]); + pH[k] = 1.0; + pH[row] = 0.0; + for (i = row - 2, pH = H + n * i; i >= 0; pH -= n, i--) { + u[0] = pH[i] - eigen_real[row]; + w[0] = pH[row]; + w[1] = 0.0; + pV = H + k * n; + for (j = k; j < row; j++, pV+=n) { + w[0] += pH[j] * pV[row - 1]; + w[1] += pH[j] * pV[row]; + } + if (eigen_imag[i] < 0.0) { + u[3] = u[0]; + v[0] = w[0]; + v[1] = w[1]; + } else { + k = i; + if (eigen_imag[i] == 0.0) { + Complex_Division(-w[0], -w[1], u[0], eigen_imag[row], &pH[row-1], + &pH[row]); + } + else { + u[1] = pH[i+1]; + u[2] = pH[n + i]; + x = eigen_real[i] - eigen_real[row]; + y = 2.0 * x * eigen_imag[row]; + x = x * x + eigen_imag[i] * eigen_imag[i] + - eigen_imag[row] * eigen_imag[row]; + if ( x == 0.0 && y == 0.0 ) + x = zero_tolerance * ( fabs(u[0]) + fabs(u[1]) + fabs(u[2]) + + fabs(u[3]) + fabs(eigen_imag[row]) ); + Complex_Division(u[1]*v[0] - u[3] * w[0] + w[1] * eigen_imag[row], + u[1] * v[1] - u[3] * w[1] - w[0] * eigen_imag[row], + x, y, &pH[row-1], &pH[row]); + if ( fabs(u[1]) > (fabs(u[3]) + fabs(eigen_imag[row])) ) { + pH[n+row-1] = -w[0] - u[0] * pH[row-1] + + eigen_imag[row] * pH[row] / u[1]; + pH[n+row] = -w[1] - u[0] * pH[row] + - eigen_imag[row] * pH[row-1] / u[1]; + } + else { + Complex_Division(-v[0] - u[2] * pH[row-1], -v[1] - u[2]*pH[row], + u[3], eigen_imag[row], &pH[n+row-1], &pH[n+row]); + } + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// static void Calculate_Eigenvectors(double *H, double *S, // +// double eigen_real[], double eigen_imag[], int n) // +// // +// Description: // +// Multiply by transformation matrix. // +// // +// Arguments: // +// double *H // +// Pointer to the first element of the matrix in Hessenberg form. // +// double *S // +// Pointer to the first element of the transformation matrix. // +// double eigen_real[] // +// The real part of an eigenvalue. // +// double eigen_imag[] // +// The imaginary part of an eigenvalue. // +// int n // +// The dimension of H, S, eigen_real, and eigen_imag. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Calculate_Eigenvectors(double *H, double *S, double eigen_real[], + double eigen_imag[], int n) +{ + double* pH; + double* pS; + double x,y; + int i,j,k; + + for (k = n-1; k >= 0; k--) { + if (eigen_imag[k] < 0.0) { + for (i = 0, pS = S; i < n; pS += n, i++) { + x = 0.0; + y = 0.0; + for (j = 0, pH = H; j <= k; pH += n, j++) { + x += pS[j] * pH[k-1]; + y += pS[j] * pH[k]; + } + pS[k-1] = x; + pS[k] = y; + } + } else if (eigen_imag[k] == 0.0) { + for (i = 0, pS = S; i < n; i++, pS += n) { + x = 0.0; + for (j = 0, pH = H; j <= k; j++, pH += n) + x += pS[j] * pH[k]; + pS[k] = x; + } + } + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// static void Complex_Division(double x, double y, double u, double v, // +// double* a, double* b) // +// // +// Description: // +// a + i b = (x + iy) / (u + iv) // +// = (x * u + y * v) / r^2 + i (y * u - x * v) / r^2, // +// where r^2 = u^2 + v^2. // +// // +// Arguments: // +// double x // +// Real part of the numerator. // +// double y // +// Imaginary part of the numerator. // +// double u // +// Real part of the denominator. // +// double v // +// Imaginary part of the denominator. // +// double *a // +// Real part of the quotient. // +// double *b // +// Imaginary part of the quotient. // +//////////////////////////////////////////////////////////////////////////////// +// // +static void Complex_Division(double x, double y, double u, double v, + double* a, double* b) +{ + double q = u*u + v*v; + + *a = (x * u + y * v) / q; + *b = (y * u - x * v) / q; +} +//////////////////////////////////////////////////////////////////////////////// +// File: transpose_square_matrix.c // +// Routine(s): // +// Transpose_Square_Matrix // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Transpose_Square_Matrix( double *A, int n ) // +// // +// Description: // +// Take the transpose of A and store in place. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int n The number of rows and columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// double A[N][N]; // +// // +// (your code to initialize the matrix A) // +// // +// Transpose_Square_Matrix( &A[0][0], N); // +// printf("The transpose of A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Transpose_Square_Matrix( double *A, int n ) +{ + double *pA, *pAt; + double temp; + int i,j; + + for (i = 0; i < n; A += n + 1, i++) { + pA = A + 1; + pAt = A + n; + for (j = i+1 ; j < n; pA++, pAt += n, j++) { + temp = *pAt; + *pAt = *pA; + *pA = temp; + } + } +} +/////////////////////////////////////////////////////////////////////////////// +// File: lower_triangular.c // +// Routines: // +// Lower_Triangular_Solve // +// Lower_Triangular_Inverse // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// int Lower_Triangular_Solve(double *L, double *B, double x[], int n) // +// // +// Description: // +// This routine solves the linear equation Lx = B, where L is an n x n // +// lower triangular matrix. (The superdiagonal part of the matrix is // +// not addressed.) // +// The algorithm follows: // +// x[0] = B[0]/L[0][0], and // +// x[i] = [B[i] - (L[i][0] * x[0] + ... + L[i][i-1] * x[i-1])] / L[i][i],// +// for i = 1, ..., n-1. // +// // +// Arguments: // +// double *L Pointer to the first element of the lower triangular // +// matrix. // +// double *B Pointer to the column vector, (n x 1) matrix, B. // +// double *x Pointer to the column vector, (n x 1) matrix, x. // +// int n The number of rows or columns of the matrix L. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Lower_Triangular_Solve(&A[0][0], B, x, n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The solution is \n"); // +// ... // +//////////////////////////////////////////////////////////////////////////////// +// // +int Lower_Triangular_Solve(double *L, double B[], double x[], int n) +{ + int i, k; + +// Solve the linear equation Lx = B for x, where L is a lower +// triangular matrix. + + for (k = 0; k < n; L += n, k++) { + if (*(L + k) == 0.0) return -1; // The matrix L is singular + x[k] = B[k]; + for (i = 0; i < k; i++) x[k] -= x[i] * *(L + i); + x[k] /= *(L + k); + } + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Lower_Triangular_Inverse(double *L, int n) // +// // +// Description: // +// This routine calculates the inverse of the lower triangular matrix L. // +// The superdiagonal part of the matrix is not addressed. // +// The algorithm follows: // +// Let M be the inverse of L, then L M = I, // +// M[i][i] = 1.0 / L[i][i] for i = 0, ..., n-1, and // +// M[i][j] = -[(L[i][j] M[j][j] + ... + L[i][i-1] M[i-1][j])] / L[i][i], // +// for i = 1, ..., n-1, j = 0, ..., i - 1. // +// // +// // +// Arguments: // +// double *L On input, the pointer to the first element of the matrix // +// whose lower triangular elements form the matrix which is // +// to be inverted. On output, the lower triangular part is // +// replaced by the inverse. The superdiagonal elements are // +// not modified. // +// its inverse. // +// int n The number of rows and/or columns of the matrix L. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix L is singular. // +// // +// Example: // +// #define N // +// double L[N][N]; // +// // +// (your code to create the matrix L) // +// err = Lower_Triangular_Inverse(&L[0][0], N); // +// if (err < 0) printf(" Matrix L is singular\n"); // +// else { // +// printf(" The inverse is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Lower_Triangular_Inverse(double *L, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + +// Invert the diagonal elements of the lower triangular matrix L. + + for (k = 0, p_k = L; k < n; p_k += (n + 1), k++) { + if (*p_k == 0.0) return -1; + else *p_k = 1.0 / *p_k; + } + +// Invert the remaining lower triangular matrix L row by row. + + for (i = 1, p_i = L + n; i < n; i++, p_i += n) { + for (j = 0, p_j = L; j < i; p_j += n, j++) { + sum = 0.0; + for (k = j, p_k = p_j; k < i; k++, p_k += n) + sum += *(p_i + k) * *(p_k + j); + *(p_i + j) = - *(p_i + i) * sum; + } + } + + return 0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: upper_triangular.c // +// Routines: // +// Upper_Triangular_Solve // +// Upper_Triangular_Inverse // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// int Upper_Triangular_Solve(double *U, double *B, double x[], int n) // +// // +// Description: // +// This routine solves the linear equation Ux = B, where U is an n x n // +// upper triangular matrix. (The subdiagonal part of the matrix is // +// not addressed.) // +// The algorithm follows: // +// x[n-1] = B[n-1]/U[n-1][n-1], and // +// x[i] = [B[i] - (U[i][i+1] * x[i+1] + ... + U[i][n-1] * x[n-1])] // +// / U[i][i], // +// for i = n-2, ..., 0. // +// // +// Arguments: // +// double *U Pointer to the first element of the upper triangular // +// matrix. // +// double *B Pointer to the column vector, (n x 1) matrix, B. // +// double *x Pointer to the column vector, (n x 1) matrix, x. // +// int n The number of rows or columns of the matrix U. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix U is singular. // +// // +// Example: // +// #define N // +// double A[N][N], B[N], x[N]; // +// // +// (your code to create matrix A and column vector B) // +// err = Upper_Triangular_Solve(&A[0][0], B, x, n); // +// if (err < 0) printf(" Matrix A is singular\n"); // +// else printf(" The solution is \n"); // +// ... // +//////////////////////////////////////////////////////////////////////////////// +// // +int Upper_Triangular_Solve(double *U, double B[], double x[], int n) +{ + int i, k; + +// Solve the linear equation Ux = B for x, where U is an upper +// triangular matrix. + + for (k = n-1, U += n * (n - 1); k >= 0; U -= n, k--) { + if (*(U + k) == 0.0) return -1; // The matrix U is singular + x[k] = B[k]; + for (i = k + 1; i < n; i++) x[k] -= x[i] * *(U + i); + x[k] /= *(U + k); + } + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// int Upper_Triangular_Inverse(double *U, int n) // +// // +// Description: // +// This routine calculates the inverse of the upper triangular matrix U. // +// The subdiagonal part of the matrix is not addressed. // +// The algorithm follows: // +// Let M be the inverse of U, then U M = I, // +// M[n-1][n-1] = 1.0 / U[n-1][n-1] and // +// M[i][j] = -( U[i][i+1] M[i+1][j] + ... + U[i][j] M[j][j] ) / U[i][i], // +// for i = n-2, ... , 0, j = n-1, ..., i+1. // +// // +// // +// Arguments: // +// double *U On input, the pointer to the first element of the matrix // +// whose upper triangular elements form the matrix which is // +// to be inverted. On output, the upper triangular part is // +// replaced by the inverse. The subdiagonal elements are // +// not modified. // +// int n The number of rows and/or columns of the matrix U. // +// // +// Return Values: // +// 0 Success // +// -1 Failure - The matrix U is singular. // +// // +// Example: // +// #define N // +// double U[N][N]; // +// // +// (your code to create the matrix U) // +// err = Upper_Triangular_Inverse(&U[0][0], N); // +// if (err < 0) printf(" Matrix U is singular\n"); // +// else { // +// printf(" The inverse is \n"); // +// ... // +// } // +//////////////////////////////////////////////////////////////////////////////// +// // +int Upper_Triangular_Inverse(double *U, int n) +{ + int i, j, k; + double *p_i, *p_j, *p_k; + double sum; + +// Invert the diagonal elements of the upper triangular matrix U. + + for (k = 0, p_k = U; k < n; p_k += (n + 1), k++) { + if (*p_k == 0.0) return -1; + else *p_k = 1.0 / *p_k; + } + +// Invert the remaining upper triangular matrix U. + + for (i = n - 2, p_i = U + n * (n - 2); i >=0; p_i -= n, i-- ) { + for (j = n - 1; j > i; j--) { + sum = 0.0; + for (k = i + 1, p_k = p_i + n; k <= j; p_k += n, k++ ) { + sum += *(p_i + k) * *(p_k + j); + } + *(p_i + j) = - *(p_i + i) * sum; + } + } + + return 0; +} +//////////////////////////////////////////////////////////////////////////////// +// File: interchange_cols.c // +// Routine(s): // +// Interchange_Columns // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Interchange_Columns(double *A, int col1, int col2, int nrows, // +// int ncols) // +// // +// Description: // +// Interchange the columns 'col1' and 'col2' of the nrows x ncols // +// matrix A. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int col1 The column of A which is to be interchanged with col2. // +// int col2 The column of A which is to be interchanged with col1. // +// int nrows The number of rows matrix A. // +// int ncols The number of columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// double A[M][N]; // +// int i,j; // +// // +// (your code to initialize the matrix A, the column number i and column // +// number j) // +// // +// if ( (i >= 0) && ( i < N ) && ( j >= 0 ) && (j < N) ) // +// Interchange_Columns(&A[0][0], i, j, M, N); // +// printf("The matrix A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Interchange_Columns(double *A, int col1, int col2, int nrows, int ncols) +{ + int i; + double *pA1, *pA2; + double temp; + + pA1 = A + col1; + pA2 = A + col2; + for (i = 0; i < nrows; pA1 += ncols, pA2 += ncols, i++) { + temp = *pA1; + *pA1 = *pA2; + *pA2 = temp; + } +} +//////////////////////////////////////////////////////////////////////////////// +// File: interchange_rows.c // +// Routine(s): // +// Interchange_Rows // +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +// void Interchange_Rows(double *A, int row1, int row2, int ncols) // +// // +// Description: // +// Interchange the rows 'row1' and 'row2' of the nrows x ncols matrix A. // +// // +// Arguments: // +// double *A Pointer to the first element of the matrix A. // +// int row1 The row of A which is to be interchanged with row row2. // +// int row2 The row of A which is to be interchanged with row row1. // +// int ncols The number of columns of the matrix A. // +// // +// Return Values: // +// void // +// // +// Example: // +// #define N // +// #define M // +// double A[M][N]; // +// int i, j; // +// // +// (your code to initialize the matrix A, the row number i and row number j) // +// // +// if ( (i >= 0) && ( i < M ) && (j > 0) && ( j < M ) ) // +// Interchange_Rows(&A[0][0], i, j, N); // +// printf("The matrix A is \n"); ... // +//////////////////////////////////////////////////////////////////////////////// +void Interchange_Rows(double *A, int row1, int row2, int ncols) +{ + int i; + double *pA1, *pA2; + double temp; + + pA1 = A + row1 * ncols; + pA2 = A + row2 * ncols; + for (i = 0; i < ncols; i++) { + temp = *pA1; + *pA1++ = *pA2; + *pA2++ = temp; + } +} \ No newline at end of file diff --git a/calibration/sample_out.txt b/calibration/sample_out.txt new file mode 100644 index 0000000..b4958ff --- /dev/null +++ b/calibration/sample_out.txt @@ -0,0 +1,34 @@ +shape of data array: (300, 3) +First 5 rows raw: + [[ 41.93 2.7 -67.14] + [ 41.77 2.97 -66.94] + [ 42.09 2.6 -66.38] + [ 42.05 3.03 -67.19] + [ 42.15 2.27 -67.33]] + +Data normalized to 1000 +Soft iron transformation matrix: + [[21.23836366 0.66439672 0.48954388] + [ 0.66439672 19.50603233 -0.03060383] + [ 0.48954388 -0.03060383 20.84084544]] +Hard iron bias: + [[ 26.9734346 ] + [ 23.62731129] + [-23.37274735]] +First 5 rows calibrated: + [[ 282.32294764 -396.9322721 -904.184197 ] + [ 279.20210535 -391.77806761 -900.10261797] + [ 286.0266995 -398.79983076 -888.26376706] + [ 285.06632501 -390.41402364 -905.17759327] + [ 286.61668372 -405.16788399 -908.02309834]] +************************* +code to paste : +************************* +float B[3] = { [26.97] , [23.63] , [-23.37] }; + + +float A_inv[3][3] = { +{ 21.23836 , 0.6644 , 0.48954 }, +{ 0.6644 , 19.50603 , -0.0306 }, +{ 0.48954 , -0.0306 , 20.84085 }}; + diff --git a/calibration/sample_serial_output.txt b/calibration/sample_serial_output.txt new file mode 100644 index 0000000..56201e7 --- /dev/null +++ b/calibration/sample_serial_output.txt @@ -0,0 +1,662 @@ +--- Terminal on /dev/ttyACM1 | 115200 8-N-1 +--- Available filters and text transformations: colorize, debug, default, direct, esp32_exception_decoder, hexlify, log2file, nocontrol, printable, send_on_enter, time +--- More details at https://bit.ly/pio-monitor-filters +--- Quit: Ctrl+C | Menu: Ctrl+T | Help: Ctrl+T followed by Ctrl+H +Found LSM9DS1 9DOF +Collecting gyro data, hold still +gyro offsets +-0.05, -0.00, 0.03 + +rotate slowly and carefully in 3D +starting +Accel X: -0.30 m/s^2 Y: 0.05 m/s^2 Z: 9.73 m/s^2 +Mag X: 37.52 uT Y: 39.80 uT Z: -55.19 uT +Accel X: -0.30 m/s^2 Y: 0.04 m/s^2 Z: 9.72 m/s^2 +Mag X: 37.21 uT Y: 39.81 uT Z: -54.79 uT +Accel X: -0.29 m/s^2 Y: 0.05 m/s^2 Z: 9.74 m/s^2 +Mag X: 37.58 uT Y: 40.43 uT Z: -55.23 uT +Accel X: -0.32 m/s^2 Y: 0.05 m/s^2 Z: 9.71 m/s^2 +Mag X: 37.65 uT Y: 39.78 uT Z: -55.58 uT +Accel X: -0.32 m/s^2 Y: 0.06 m/s^2 Z: 9.69 m/s^2 +Mag X: 37.45 uT Y: 39.81 uT Z: -55.63 uT +Accel X: -0.33 m/s^2 Y: 0.08 m/s^2 Z: 9.69 m/s^2 +Mag X: 37.65 uT Y: 39.87 uT Z: -55.16 uT +Accel X: -0.32 m/s^2 Y: 0.07 m/s^2 Z: 9.73 m/s^2 +Mag X: 37.43 uT Y: 39.93 uT Z: -55.14 uT +Accel X: -0.30 m/s^2 Y: 0.05 m/s^2 Z: 9.74 m/s^2 +Mag X: 37.61 uT Y: 40.02 uT Z: -55.74 uT +Accel X: -0.34 m/s^2 Y: 0.06 m/s^2 Z: 9.72 m/s^2 +Mag X: 37.18 uT Y: 40.49 uT Z: -55.58 uT +Accel X: -0.34 m/s^2 Y: 0.06 m/s^2 Z: 9.72 m/s^2 +Mag X: 37.62 uT Y: 40.13 uT Z: -55.63 uT +Accel X: -0.29 m/s^2 Y: 0.08 m/s^2 Z: 9.72 m/s^2 +Mag X: 37.46 uT Y: 40.49 uT Z: -55.36 uT +Accel X: -0.36 m/s^2 Y: 0.08 m/s^2 Z: 9.66 m/s^2 +Mag X: 37.59 uT Y: 40.34 uT Z: -55.67 uT +Accel X: 0.09 m/s^2 Y: 0.24 m/s^2 Z: 9.69 m/s^2 +Mag X: 37.53 uT Y: 40.62 uT Z: -55.55 uT +Accel X: -0.24 m/s^2 Y: 0.10 m/s^2 Z: 9.44 m/s^2 +Mag X: 37.77 uT Y: 39.93 uT Z: -55.66 uT +Accel X: -0.49 m/s^2 Y: 0.08 m/s^2 Z: 9.68 m/s^2 +Mag X: 38.03 uT Y: 40.47 uT Z: -56.64 uT +Accel X: -0.49 m/s^2 Y: -0.75 m/s^2 Z: 10.05 m/s^2 +Mag X: 38.88 uT Y: 39.94 uT Z: -55.39 uT +Accel X: 0.13 m/s^2 Y: 0.09 m/s^2 Z: 10.36 m/s^2 +Mag X: 39.78 uT Y: 39.23 uT Z: -54.57 uT +Accel X: -0.52 m/s^2 Y: -0.24 m/s^2 Z: 9.72 m/s^2 +Mag X: 39.93 uT Y: 38.78 uT Z: -54.52 uT +Accel X: -0.00 m/s^2 Y: 0.07 m/s^2 Z: 9.96 m/s^2 +Mag X: 42.12 uT Y: 40.73 uT Z: -53.84 uT +Accel X: 0.74 m/s^2 Y: -1.88 m/s^2 Z: 9.26 m/s^2 +Mag X: 44.68 uT Y: 44.31 uT Z: -51.11 uT +Accel X: 0.89 m/s^2 Y: -2.00 m/s^2 Z: 9.34 m/s^2 +Mag X: 44.86 uT Y: 47.56 uT Z: -48.45 uT +Accel X: 1.05 m/s^2 Y: -3.62 m/s^2 Z: 9.00 m/s^2 +Mag X: 46.19 uT Y: 51.39 uT Z: -44.94 uT +Accel X: 0.84 m/s^2 Y: -5.05 m/s^2 Z: 8.23 m/s^2 +Mag X: 46.77 uT Y: 55.06 uT Z: -40.63 uT +Accel X: 1.03 m/s^2 Y: -5.86 m/s^2 Z: 7.99 m/s^2 +Mag X: 45.95 uT Y: 58.13 uT Z: -37.69 uT +Accel X: 0.58 m/s^2 Y: -6.90 m/s^2 Z: 6.46 m/s^2 +Mag X: 44.15 uT Y: 61.90 uT Z: -33.10 uT +Accel X: 0.57 m/s^2 Y: -7.80 m/s^2 Z: 5.72 m/s^2 +Mag X: 43.19 uT Y: 64.59 uT Z: -26.75 uT +Accel X: 0.67 m/s^2 Y: -8.31 m/s^2 Z: 5.04 m/s^2 +Mag X: 42.36 uT Y: 66.34 uT Z: -20.64 uT +Accel X: 0.31 m/s^2 Y: -8.80 m/s^2 Z: 3.14 m/s^2 +Mag X: 41.60 uT Y: 67.03 uT Z: -15.27 uT +Accel X: 0.20 m/s^2 Y: -9.79 m/s^2 Z: 1.81 m/s^2 +Mag X: 40.89 uT Y: 67.03 uT Z: -8.48 uT +Accel X: -0.15 m/s^2 Y: -9.31 m/s^2 Z: 0.37 m/s^2 +Mag X: 40.40 uT Y: 66.30 uT Z: -5.01 uT +Accel X: 0.02 m/s^2 Y: -9.79 m/s^2 Z: 0.94 m/s^2 +Mag X: 40.89 uT Y: 65.32 uT Z: -2.43 uT +Accel X: 0.11 m/s^2 Y: -9.90 m/s^2 Z: -1.04 m/s^2 +Mag X: 40.81 uT Y: 64.43 uT Z: 0.04 uT +Accel X: -0.46 m/s^2 Y: -9.30 m/s^2 Z: -1.94 m/s^2 +Mag X: 40.70 uT Y: 62.60 uT Z: 3.73 uT +Accel X: -0.37 m/s^2 Y: -9.12 m/s^2 Z: -1.94 m/s^2 +Mag X: 39.42 uT Y: 60.67 uT Z: 7.97 uT +Accel X: -0.42 m/s^2 Y: -8.95 m/s^2 Z: -4.12 m/s^2 +Mag X: 39.48 uT Y: 59.00 uT Z: 10.36 uT +Accel X: -0.48 m/s^2 Y: -9.27 m/s^2 Z: -4.22 m/s^2 +Mag X: 39.83 uT Y: 56.23 uT Z: 13.24 uT +Accel X: -0.22 m/s^2 Y: -9.00 m/s^2 Z: -4.31 m/s^2 +Mag X: 41.63 uT Y: 52.95 uT Z: 14.37 uT +Accel X: -0.13 m/s^2 Y: -8.03 m/s^2 Z: -5.64 m/s^2 +Mag X: 42.62 uT Y: 50.61 uT Z: 17.44 uT +Accel X: -0.07 m/s^2 Y: -8.60 m/s^2 Z: -5.46 m/s^2 +Mag X: 42.71 uT Y: 48.26 uT Z: 18.61 uT +Accel X: 0.14 m/s^2 Y: -7.66 m/s^2 Z: -6.61 m/s^2 +Mag X: 42.49 uT Y: 46.86 uT Z: 19.50 uT +Accel X: 0.66 m/s^2 Y: -7.77 m/s^2 Z: -6.47 m/s^2 +Mag X: 42.87 uT Y: 44.96 uT Z: 21.78 uT +Accel X: 0.40 m/s^2 Y: -6.81 m/s^2 Z: -8.10 m/s^2 +Mag X: 42.77 uT Y: 42.31 uT Z: 22.80 uT +Accel X: 0.53 m/s^2 Y: -6.35 m/s^2 Z: -7.98 m/s^2 +Mag X: 44.31 uT Y: 39.87 uT Z: 22.67 uT +Accel X: 0.24 m/s^2 Y: -5.39 m/s^2 Z: -8.44 m/s^2 +Mag X: 44.64 uT Y: 34.84 uT Z: 25.10 uT +Accel X: 0.68 m/s^2 Y: -3.37 m/s^2 Z: -9.18 m/s^2 +Mag X: 45.18 uT Y: 28.75 uT Z: 25.99 uT +Accel X: 0.42 m/s^2 Y: -2.70 m/s^2 Z: -9.59 m/s^2 +Mag X: 44.78 uT Y: 22.51 uT Z: 26.44 uT +Accel X: 0.54 m/s^2 Y: -1.58 m/s^2 Z: -10.85 m/s^2 +Mag X: 43.57 uT Y: 15.89 uT Z: 26.31 uT +Accel X: 0.57 m/s^2 Y: 0.95 m/s^2 Z: -10.54 m/s^2 +Mag X: 42.34 uT Y: 9.02 uT Z: 24.25 uT +Accel X: 0.48 m/s^2 Y: 2.17 m/s^2 Z: -9.95 m/s^2 +Mag X: 41.42 uT Y: 1.24 uT Z: 21.66 uT +Accel X: 0.03 m/s^2 Y: 3.97 m/s^2 Z: -9.63 m/s^2 +Mag X: 42.55 uT Y: -5.58 uT Z: 18.01 uT +Accel X: 0.18 m/s^2 Y: 5.08 m/s^2 Z: -8.48 m/s^2 +Mag X: 42.98 uT Y: -10.01 uT Z: 12.72 uT +Accel X: 0.25 m/s^2 Y: 5.88 m/s^2 Z: -7.63 m/s^2 +Mag X: 43.26 uT Y: -13.74 uT Z: 7.53 uT +Accel X: 0.69 m/s^2 Y: 6.63 m/s^2 Z: -7.54 m/s^2 +Mag X: 44.72 uT Y: -16.54 uT Z: 3.08 uT +Accel X: 0.68 m/s^2 Y: 8.28 m/s^2 Z: -5.86 m/s^2 +Mag X: 43.63 uT Y: -19.20 uT Z: -3.46 uT +Accel X: 0.08 m/s^2 Y: 9.16 m/s^2 Z: -3.11 m/s^2 +Mag X: 42.65 uT Y: -20.83 uT Z: -10.11 uT +Accel X: 0.55 m/s^2 Y: 10.00 m/s^2 Z: -1.86 m/s^2 +Mag X: 42.82 uT Y: -20.84 uT Z: -20.55 uT +Accel X: -0.13 m/s^2 Y: 9.72 m/s^2 Z: 0.74 m/s^2 +Mag X: 41.20 uT Y: -18.69 uT Z: -29.67 uT +Accel X: -0.34 m/s^2 Y: 9.34 m/s^2 Z: 2.26 m/s^2 +Mag X: 41.06 uT Y: -14.25 uT Z: -37.15 uT +Accel X: 0.17 m/s^2 Y: 8.64 m/s^2 Z: 4.07 m/s^2 +Mag X: 41.54 uT Y: -10.08 uT Z: -43.45 uT +Accel X: -0.40 m/s^2 Y: 8.46 m/s^2 Z: 5.29 m/s^2 +Mag X: 41.52 uT Y: -5.63 uT Z: -46.29 uT +Accel X: -0.15 m/s^2 Y: 7.33 m/s^2 Z: 5.57 m/s^2 +Mag X: 42.01 uT Y: 0.58 uT Z: -51.17 uT +Accel X: -0.25 m/s^2 Y: 6.59 m/s^2 Z: 7.36 m/s^2 +Mag X: 41.27 uT Y: 5.50 uT Z: -53.64 uT +Accel X: -0.66 m/s^2 Y: 5.44 m/s^2 Z: 8.00 m/s^2 +Mag X: 40.32 uT Y: 10.93 uT Z: -54.90 uT +Accel X: -0.23 m/s^2 Y: 4.31 m/s^2 Z: 8.85 m/s^2 +Mag X: 39.23 uT Y: 17.74 uT Z: -56.64 uT +Accel X: -0.22 m/s^2 Y: 2.93 m/s^2 Z: 9.21 m/s^2 +Mag X: 38.38 uT Y: 22.76 uT Z: -57.47 uT +Accel X: 0.07 m/s^2 Y: 1.47 m/s^2 Z: 10.09 m/s^2 +Mag X: 37.72 uT Y: 27.62 uT Z: -57.04 uT +Accel X: -0.21 m/s^2 Y: 1.41 m/s^2 Z: 9.93 m/s^2 +Mag X: 38.79 uT Y: 33.35 uT Z: -55.19 uT +Accel X: -0.23 m/s^2 Y: 0.88 m/s^2 Z: 9.76 m/s^2 +Mag X: 39.70 uT Y: 34.96 uT Z: -54.21 uT +Accel X: -0.19 m/s^2 Y: 1.28 m/s^2 Z: 10.10 m/s^2 +Mag X: 39.93 uT Y: 32.64 uT Z: -55.35 uT +Accel X: 0.15 m/s^2 Y: 2.71 m/s^2 Z: 10.74 m/s^2 +Mag X: 40.03 uT Y: 26.75 uT Z: -55.60 uT +Accel X: -0.38 m/s^2 Y: 3.48 m/s^2 Z: 9.24 m/s^2 +Mag X: 40.46 uT Y: 23.50 uT Z: -56.02 uT +Accel X: -0.96 m/s^2 Y: 4.06 m/s^2 Z: 8.62 m/s^2 +Mag X: 39.54 uT Y: 17.87 uT Z: -55.45 uT +Accel X: -0.58 m/s^2 Y: 6.27 m/s^2 Z: 7.41 m/s^2 +Mag X: 39.64 uT Y: 9.12 uT Z: -53.86 uT +Accel X: -0.27 m/s^2 Y: 7.50 m/s^2 Z: 6.48 m/s^2 +Mag X: 40.05 uT Y: 0.76 uT Z: -51.36 uT +Accel X: -0.58 m/s^2 Y: 8.22 m/s^2 Z: 4.97 m/s^2 +Mag X: 40.91 uT Y: -6.75 uT Z: -45.95 uT +Accel X: 0.12 m/s^2 Y: 9.99 m/s^2 Z: 1.76 m/s^2 +Mag X: 42.84 uT Y: -14.22 uT Z: -37.42 uT +Accel X: 0.67 m/s^2 Y: 9.76 m/s^2 Z: 0.39 m/s^2 +Mag X: 46.35 uT Y: -16.72 uT Z: -31.45 uT +Accel X: 1.82 m/s^2 Y: 10.07 m/s^2 Z: -0.34 m/s^2 +Mag X: 50.10 uT Y: -16.82 uT Z: -27.24 uT +Accel X: 2.61 m/s^2 Y: 8.73 m/s^2 Z: -0.60 m/s^2 +Mag X: 54.00 uT Y: -15.77 uT Z: -23.85 uT +Accel X: 4.70 m/s^2 Y: 8.55 m/s^2 Z: -2.66 m/s^2 +Mag X: 57.23 uT Y: -14.31 uT Z: -22.04 uT +Accel X: 5.70 m/s^2 Y: 7.97 m/s^2 Z: -3.01 m/s^2 +Mag X: 59.89 uT Y: -12.03 uT Z: -19.10 uT +Accel X: 6.00 m/s^2 Y: 7.75 m/s^2 Z: -4.55 m/s^2 +Mag X: 62.89 uT Y: -9.31 uT Z: -17.68 uT +Accel X: 6.54 m/s^2 Y: 4.34 m/s^2 Z: -3.38 m/s^2 +Mag X: 64.56 uT Y: -7.22 uT Z: -12.99 uT +Accel X: 7.43 m/s^2 Y: 4.01 m/s^2 Z: -4.70 m/s^2 +Mag X: 66.53 uT Y: -4.21 uT Z: -11.01 uT +Accel X: 7.58 m/s^2 Y: 2.67 m/s^2 Z: -5.05 m/s^2 +Mag X: 67.96 uT Y: -0.99 uT Z: -9.41 uT +Accel X: 7.51 m/s^2 Y: 0.47 m/s^2 Z: -4.80 m/s^2 +Mag X: 69.19 uT Y: 3.71 uT Z: -6.66 uT +Accel X: 8.67 m/s^2 Y: -0.40 m/s^2 Z: -4.95 m/s^2 +Mag X: 71.41 uT Y: 8.89 uT Z: -5.17 uT +Accel X: 8.38 m/s^2 Y: -1.53 m/s^2 Z: -4.28 m/s^2 +Mag X: 72.07 uT Y: 12.98 uT Z: -6.77 uT +Accel X: 8.82 m/s^2 Y: -3.31 m/s^2 Z: -3.39 m/s^2 +Mag X: 73.37 uT Y: 18.63 uT Z: -7.97 uT +Accel X: 8.72 m/s^2 Y: -4.05 m/s^2 Z: -1.45 m/s^2 +Mag X: 73.17 uT Y: 23.66 uT Z: -11.08 uT +Accel X: 7.77 m/s^2 Y: -5.22 m/s^2 Z: -0.10 m/s^2 +Mag X: 72.49 uT Y: 29.10 uT Z: -15.05 uT +Accel X: 7.24 m/s^2 Y: -6.83 m/s^2 Z: 1.29 m/s^2 +Mag X: 71.46 uT Y: 34.16 uT Z: -18.71 uT +Accel X: 6.41 m/s^2 Y: -7.40 m/s^2 Z: 2.74 m/s^2 +Mag X: 69.44 uT Y: 36.35 uT Z: -24.48 uT +Accel X: 6.33 m/s^2 Y: -5.48 m/s^2 Z: 3.98 m/s^2 +Mag X: 68.40 uT Y: 37.36 uT Z: -27.49 uT +Accel X: 6.85 m/s^2 Y: -5.06 m/s^2 Z: 4.99 m/s^2 +Mag X: 67.86 uT Y: 34.86 uT Z: -31.41 uT +Accel X: 6.03 m/s^2 Y: -4.03 m/s^2 Z: 5.73 m/s^2 +Mag X: 66.68 uT Y: 32.04 uT Z: -34.61 uT +Accel X: 5.65 m/s^2 Y: -4.21 m/s^2 Z: 6.72 m/s^2 +Mag X: 66.12 uT Y: 29.51 uT Z: -36.45 uT +Accel X: 5.66 m/s^2 Y: -1.10 m/s^2 Z: 8.00 m/s^2 +Mag X: 65.71 uT Y: 31.23 uT Z: -35.40 uT +Accel X: 4.56 m/s^2 Y: -3.21 m/s^2 Z: 6.53 m/s^2 +Mag X: 64.83 uT Y: 30.43 uT Z: -37.46 uT +Accel X: 4.44 m/s^2 Y: -2.40 m/s^2 Z: 8.15 m/s^2 +Mag X: 63.78 uT Y: 27.57 uT Z: -40.34 uT +Accel X: 2.82 m/s^2 Y: -2.57 m/s^2 Z: 6.61 m/s^2 +Mag X: 59.88 uT Y: 26.41 uT Z: -45.35 uT +Accel X: 3.30 m/s^2 Y: -2.79 m/s^2 Z: 9.09 m/s^2 +Mag X: 57.02 uT Y: 26.00 uT Z: -47.03 uT +Accel X: 2.42 m/s^2 Y: -1.66 m/s^2 Z: 9.81 m/s^2 +Mag X: 54.30 uT Y: 26.22 uT Z: -48.79 uT +Accel X: 0.29 m/s^2 Y: -0.24 m/s^2 Z: 9.60 m/s^2 +Mag X: 49.82 uT Y: 28.15 uT Z: -52.41 uT +Accel X: -1.09 m/s^2 Y: -2.52 m/s^2 Z: 9.80 m/s^2 +Mag X: 41.95 uT Y: 33.43 uT Z: -54.78 uT +Accel X: -2.65 m/s^2 Y: -2.08 m/s^2 Z: 9.71 m/s^2 +Mag X: 34.03 uT Y: 35.88 uT Z: -55.10 uT +Accel X: -3.57 m/s^2 Y: -1.30 m/s^2 Z: 8.46 m/s^2 +Mag X: 29.20 uT Y: 37.93 uT Z: -55.39 uT +Accel X: -4.99 m/s^2 Y: -2.28 m/s^2 Z: 8.14 m/s^2 +Mag X: 25.29 uT Y: 40.53 uT Z: -54.09 uT +Accel X: -5.15 m/s^2 Y: -3.17 m/s^2 Z: 8.21 m/s^2 +Mag X: 22.89 uT Y: 42.69 uT Z: -53.62 uT +Accel X: -5.57 m/s^2 Y: -3.09 m/s^2 Z: 8.24 m/s^2 +Mag X: 21.16 uT Y: 43.57 uT Z: -52.97 uT +Accel X: -6.50 m/s^2 Y: -2.51 m/s^2 Z: 6.42 m/s^2 +Mag X: 17.16 uT Y: 43.60 uT Z: -52.46 uT +Accel X: -6.42 m/s^2 Y: -1.33 m/s^2 Z: 5.00 m/s^2 +Mag X: 10.16 uT Y: 43.79 uT Z: -49.24 uT +Accel X: -7.73 m/s^2 Y: -3.01 m/s^2 Z: 5.09 m/s^2 +Mag X: 5.64 uT Y: 45.48 uT Z: -46.20 uT +Accel X: -8.04 m/s^2 Y: -2.55 m/s^2 Z: 3.29 m/s^2 +Mag X: 1.07 uT Y: 46.14 uT Z: -42.78 uT +Accel X: -9.02 m/s^2 Y: -3.43 m/s^2 Z: 2.93 m/s^2 +Mag X: -2.18 uT Y: 47.41 uT Z: -39.84 uT +Accel X: -8.96 m/s^2 Y: -3.42 m/s^2 Z: 2.52 m/s^2 +Mag X: -3.64 uT Y: 48.64 uT Z: -36.54 uT +Accel X: -9.71 m/s^2 Y: -3.60 m/s^2 Z: 1.30 m/s^2 +Mag X: -4.50 uT Y: 50.66 uT Z: -32.67 uT +Accel X: -9.88 m/s^2 Y: -3.72 m/s^2 Z: 0.58 m/s^2 +Mag X: -5.55 uT Y: 52.60 uT Z: -30.30 uT +Accel X: -9.35 m/s^2 Y: -3.13 m/s^2 Z: -1.64 m/s^2 +Mag X: -4.63 uT Y: 54.47 uT Z: -27.92 uT +Accel X: -8.55 m/s^2 Y: -4.37 m/s^2 Z: -0.41 m/s^2 +Mag X: -3.68 uT Y: 57.61 uT Z: -25.39 uT +Accel X: -8.43 m/s^2 Y: -5.44 m/s^2 Z: -1.92 m/s^2 +Mag X: -3.74 uT Y: 58.67 uT Z: -21.82 uT +Accel X: -7.84 m/s^2 Y: -5.96 m/s^2 Z: -2.35 m/s^2 +Mag X: -1.45 uT Y: 60.79 uT Z: -18.68 uT +Accel X: -6.70 m/s^2 Y: -5.59 m/s^2 Z: -3.63 m/s^2 +Mag X: 1.84 uT Y: 62.57 uT Z: -14.59 uT +Accel X: -7.10 m/s^2 Y: -6.50 m/s^2 Z: -4.46 m/s^2 +Mag X: 6.02 uT Y: 64.72 uT Z: -11.40 uT +Accel X: -6.00 m/s^2 Y: -5.64 m/s^2 Z: -4.94 m/s^2 +Mag X: 7.03 uT Y: 64.50 uT Z: -9.81 uT +Accel X: -5.61 m/s^2 Y: -6.32 m/s^2 Z: -5.42 m/s^2 +Mag X: 7.88 uT Y: 64.62 uT Z: -9.75 uT +Accel X: -5.84 m/s^2 Y: -6.50 m/s^2 Z: -4.85 m/s^2 +Mag X: 8.32 uT Y: 64.54 uT Z: -12.07 uT +Accel X: -5.70 m/s^2 Y: -6.41 m/s^2 Z: -5.27 m/s^2 +Mag X: 3.81 uT Y: 64.45 uT Z: -12.20 uT +Accel X: -5.36 m/s^2 Y: -6.88 m/s^2 Z: -4.88 m/s^2 +Mag X: 8.18 uT Y: 64.86 uT Z: -11.59 uT +Accel X: -5.20 m/s^2 Y: -6.87 m/s^2 Z: -4.97 m/s^2 +Mag X: 9.63 uT Y: 65.05 uT Z: -11.03 uT +Accel X: -3.86 m/s^2 Y: -6.97 m/s^2 Z: -5.92 m/s^2 +Mag X: 12.72 uT Y: 65.78 uT Z: -10.80 uT +Accel X: -3.33 m/s^2 Y: -6.96 m/s^2 Z: -5.20 m/s^2 +Mag X: 16.57 uT Y: 66.30 uT Z: -9.47 uT +Accel X: -3.49 m/s^2 Y: -6.50 m/s^2 Z: -6.21 m/s^2 +Mag X: 19.56 uT Y: 64.95 uT Z: -6.56 uT +Accel X: -2.41 m/s^2 Y: -5.61 m/s^2 Z: -7.34 m/s^2 +Mag X: 23.24 uT Y: 63.64 uT Z: -2.44 uT +Accel X: -1.97 m/s^2 Y: -5.26 m/s^2 Z: -8.18 m/s^2 +Mag X: 26.95 uT Y: 62.41 uT Z: -0.48 uT +Accel X: -1.45 m/s^2 Y: -4.90 m/s^2 Z: -8.68 m/s^2 +Mag X: 28.60 uT Y: 60.93 uT Z: 1.36 uT +Accel X: -1.53 m/s^2 Y: -4.31 m/s^2 Z: -9.27 m/s^2 +Mag X: 30.31 uT Y: 60.23 uT Z: 2.25 uT +Accel X: -0.56 m/s^2 Y: -3.27 m/s^2 Z: -9.47 m/s^2 +Mag X: 31.91 uT Y: 58.80 uT Z: 3.68 uT +Accel X: -0.68 m/s^2 Y: -3.18 m/s^2 Z: -8.70 m/s^2 +Mag X: 32.71 uT Y: 57.70 uT Z: 4.46 uT +Accel X: -0.93 m/s^2 Y: -2.07 m/s^2 Z: -9.66 m/s^2 +Mag X: 34.11 uT Y: 57.56 uT Z: 3.62 uT +Accel X: -0.85 m/s^2 Y: -3.00 m/s^2 Z: -9.87 m/s^2 +Mag X: 35.68 uT Y: 57.82 uT Z: 1.20 uT +Accel X: -0.81 m/s^2 Y: -3.69 m/s^2 Z: -9.42 m/s^2 +Mag X: 34.77 uT Y: 57.75 uT Z: 1.51 uT +Accel X: -1.76 m/s^2 Y: -4.89 m/s^2 Z: -7.95 m/s^2 +Mag X: 32.43 uT Y: 59.18 uT Z: -2.85 uT +Accel X: -2.90 m/s^2 Y: -6.37 m/s^2 Z: -6.31 m/s^2 +Mag X: 29.39 uT Y: 61.24 uT Z: -4.40 uT +Accel X: -2.99 m/s^2 Y: -7.43 m/s^2 Z: -4.62 m/s^2 +Mag X: 25.53 uT Y: 63.78 uT Z: -8.24 uT +Accel X: -4.81 m/s^2 Y: -8.65 m/s^2 Z: -3.12 m/s^2 +Mag X: 21.94 uT Y: 66.56 uT Z: -16.41 uT +Accel X: -5.84 m/s^2 Y: -8.84 m/s^2 Z: -1.19 m/s^2 +Mag X: 18.40 uT Y: 65.84 uT Z: -23.79 uT +Accel X: -6.75 m/s^2 Y: -9.35 m/s^2 Z: 2.58 m/s^2 +Mag X: 16.09 uT Y: 62.39 uT Z: -33.19 uT +Accel X: -6.76 m/s^2 Y: -7.00 m/s^2 Z: 5.87 m/s^2 +Mag X: 17.07 uT Y: 54.75 uT Z: -45.07 uT +Accel X: -5.68 m/s^2 Y: -2.55 m/s^2 Z: 7.56 m/s^2 +Mag X: 20.27 uT Y: 43.70 uT Z: -52.73 uT +Accel X: -5.08 m/s^2 Y: -2.38 m/s^2 Z: 7.70 m/s^2 +Mag X: 23.97 uT Y: 37.91 uT Z: -55.50 uT +Accel X: -3.08 m/s^2 Y: -3.23 m/s^2 Z: 9.12 m/s^2 +Mag X: 29.32 uT Y: 34.99 uT Z: -56.87 uT +Accel X: -1.37 m/s^2 Y: -1.55 m/s^2 Z: 9.67 m/s^2 +Mag X: 37.04 uT Y: 29.80 uT Z: -57.02 uT +Accel X: -0.32 m/s^2 Y: -0.55 m/s^2 Z: 9.05 m/s^2 +Mag X: 44.71 uT Y: 25.81 uT Z: -55.33 uT +Accel X: 0.47 m/s^2 Y: -0.62 m/s^2 Z: 9.97 m/s^2 +Mag X: 48.77 uT Y: 23.76 uT Z: -53.83 uT +Accel X: 2.01 m/s^2 Y: -0.18 m/s^2 Z: 10.51 m/s^2 +Mag X: 53.98 uT Y: 19.99 uT Z: -50.25 uT +Accel X: 4.36 m/s^2 Y: -0.53 m/s^2 Z: 9.33 m/s^2 +Mag X: 58.42 uT Y: 20.93 uT Z: -46.04 uT +Accel X: 6.14 m/s^2 Y: -0.25 m/s^2 Z: 7.81 m/s^2 +Mag X: 64.75 uT Y: 21.28 uT Z: -40.54 uT +Accel X: 6.90 m/s^2 Y: -1.00 m/s^2 Z: 8.22 m/s^2 +Mag X: 69.76 uT Y: 22.76 uT Z: -31.96 uT +Accel X: 8.14 m/s^2 Y: 0.02 m/s^2 Z: 5.18 m/s^2 +Mag X: 71.63 uT Y: 24.42 uT Z: -25.77 uT +Accel X: 9.29 m/s^2 Y: -0.49 m/s^2 Z: 4.16 m/s^2 +Mag X: 72.96 uT Y: 25.29 uT Z: -12.77 uT +Accel X: 9.54 m/s^2 Y: -0.87 m/s^2 Z: 0.86 m/s^2 +Mag X: 71.41 uT Y: 23.00 uT Z: -3.26 uT +Accel X: 9.73 m/s^2 Y: -1.01 m/s^2 Z: -0.71 m/s^2 +Mag X: 66.09 uT Y: 20.93 uT Z: 8.97 uT +Accel X: 7.37 m/s^2 Y: -0.27 m/s^2 Z: -4.80 m/s^2 +Mag X: 59.56 uT Y: 19.07 uT Z: 17.23 uT +Accel X: 6.96 m/s^2 Y: -0.15 m/s^2 Z: -6.73 m/s^2 +Mag X: 50.66 uT Y: 18.59 uT Z: 22.93 uT +Accel X: 4.79 m/s^2 Y: 0.03 m/s^2 Z: -7.30 m/s^2 +Mag X: 40.66 uT Y: 18.50 uT Z: 26.92 uT +Accel X: 3.87 m/s^2 Y: 0.03 m/s^2 Z: -8.95 m/s^2 +Mag X: 32.13 uT Y: 18.94 uT Z: 28.69 uT +Accel X: 3.06 m/s^2 Y: 0.42 m/s^2 Z: -10.11 m/s^2 +Mag X: 24.92 uT Y: 19.38 uT Z: 29.07 uT +Accel X: 1.36 m/s^2 Y: 0.70 m/s^2 Z: -9.65 m/s^2 +Mag X: 17.42 uT Y: 18.39 uT Z: 27.49 uT +Accel X: -0.06 m/s^2 Y: 0.20 m/s^2 Z: -10.17 m/s^2 +Mag X: 11.79 uT Y: 17.82 uT Z: 25.78 uT +Accel X: -0.98 m/s^2 Y: 0.61 m/s^2 Z: -9.63 m/s^2 +Mag X: 7.44 uT Y: 18.06 uT Z: 23.55 uT +Accel X: -2.43 m/s^2 Y: 1.18 m/s^2 Z: -10.33 m/s^2 +Mag X: 3.90 uT Y: 16.46 uT Z: 22.10 uT +Accel X: -2.91 m/s^2 Y: 0.40 m/s^2 Z: -9.80 m/s^2 +Mag X: 0.99 uT Y: 15.48 uT Z: 18.31 uT +Accel X: -3.69 m/s^2 Y: 0.47 m/s^2 Z: -8.94 m/s^2 +Mag X: -1.67 uT Y: 15.04 uT Z: 16.91 uT +Accel X: -4.02 m/s^2 Y: 0.91 m/s^2 Z: -10.52 m/s^2 +Mag X: -3.36 uT Y: 14.18 uT Z: 14.15 uT +Accel X: -4.21 m/s^2 Y: 0.22 m/s^2 Z: -9.11 m/s^2 +Mag X: -5.17 uT Y: 14.67 uT Z: 13.52 uT +Accel X: -4.69 m/s^2 Y: 0.17 m/s^2 Z: -8.48 m/s^2 +Mag X: -5.50 uT Y: 15.49 uT Z: 13.37 uT +Accel X: -5.02 m/s^2 Y: 0.26 m/s^2 Z: -8.67 m/s^2 +Mag X: -5.63 uT Y: 16.16 uT Z: 12.38 uT +Accel X: -5.89 m/s^2 Y: 0.15 m/s^2 Z: -8.08 m/s^2 +Mag X: -5.77 uT Y: 16.50 uT Z: 12.28 uT +Accel X: -5.68 m/s^2 Y: -0.03 m/s^2 Z: -7.37 m/s^2 +Mag X: -6.61 uT Y: 16.65 uT Z: 12.36 uT +Accel X: -5.16 m/s^2 Y: 0.28 m/s^2 Z: -8.46 m/s^2 +Mag X: -6.43 uT Y: 16.38 uT Z: 11.58 uT +Accel X: -5.41 m/s^2 Y: -0.85 m/s^2 Z: -8.63 m/s^2 +Mag X: -6.61 uT Y: 16.54 uT Z: 11.68 uT +Accel X: -5.53 m/s^2 Y: -1.01 m/s^2 Z: -8.82 m/s^2 +Mag X: -6.97 uT Y: 16.22 uT Z: 11.47 uT +Accel X: -5.93 m/s^2 Y: -0.83 m/s^2 Z: -7.74 m/s^2 +Mag X: -7.23 uT Y: 16.18 uT Z: 10.79 uT +Accel X: -5.71 m/s^2 Y: -1.30 m/s^2 Z: -8.48 m/s^2 +Mag X: -7.32 uT Y: 15.74 uT Z: 10.95 uT +Accel X: -5.38 m/s^2 Y: -0.52 m/s^2 Z: -8.47 m/s^2 +Mag X: -6.66 uT Y: 12.66 uT Z: 10.08 uT +Accel X: -5.93 m/s^2 Y: 0.24 m/s^2 Z: -8.74 m/s^2 +Mag X: -7.10 uT Y: 10.82 uT Z: 7.94 uT +Accel X: -8.21 m/s^2 Y: 1.55 m/s^2 Z: -6.78 m/s^2 +Mag X: -8.26 uT Y: 7.07 uT Z: 2.28 uT +Accel X: -7.34 m/s^2 Y: 1.70 m/s^2 Z: -6.24 m/s^2 +Mag X: -7.95 uT Y: 2.85 uT Z: -3.73 uT +Accel X: -7.84 m/s^2 Y: 3.72 m/s^2 Z: -5.12 m/s^2 +Mag X: -9.53 uT Y: 3.01 uT Z: -7.22 uT +Accel X: -8.44 m/s^2 Y: 4.31 m/s^2 Z: -4.67 m/s^2 +Mag X: -9.41 uT Y: 2.70 uT Z: -13.14 uT +Accel X: -8.64 m/s^2 Y: 4.59 m/s^2 Z: -3.13 m/s^2 +Mag X: -8.67 uT Y: 0.41 uT Z: -17.77 uT +Accel X: -8.36 m/s^2 Y: 5.78 m/s^2 Z: -1.37 m/s^2 +Mag X: -7.06 uT Y: -0.23 uT Z: -23.21 uT +Accel X: -8.06 m/s^2 Y: 5.31 m/s^2 Z: 0.17 m/s^2 +Mag X: -2.56 uT Y: -2.43 uT Z: -31.98 uT +Accel X: -7.45 m/s^2 Y: 6.60 m/s^2 Z: 1.41 m/s^2 +Mag X: 2.98 uT Y: -3.51 uT Z: -37.65 uT +Accel X: -6.14 m/s^2 Y: 5.79 m/s^2 Z: 2.70 m/s^2 +Mag X: 7.97 uT Y: -2.10 uT Z: -43.07 uT +Accel X: -6.24 m/s^2 Y: 7.30 m/s^2 Z: 3.93 m/s^2 +Mag X: 12.95 uT Y: -0.80 uT Z: -47.02 uT +Accel X: -5.68 m/s^2 Y: 6.08 m/s^2 Z: 3.66 m/s^2 +Mag X: 14.10 uT Y: 1.49 uT Z: -48.41 uT +Accel X: -5.56 m/s^2 Y: 6.28 m/s^2 Z: 6.81 m/s^2 +Mag X: 18.74 uT Y: 4.18 uT Z: -51.83 uT +Accel X: -5.08 m/s^2 Y: 4.38 m/s^2 Z: 6.14 m/s^2 +Mag X: 21.38 uT Y: 9.95 uT Z: -55.35 uT +Accel X: -4.12 m/s^2 Y: 3.81 m/s^2 Z: 7.91 m/s^2 +Mag X: 25.86 uT Y: 14.85 uT Z: -56.94 uT +Accel X: -3.44 m/s^2 Y: 3.15 m/s^2 Z: 8.44 m/s^2 +Mag X: 29.25 uT Y: 18.04 uT Z: -57.76 uT +Accel X: -2.64 m/s^2 Y: 1.97 m/s^2 Z: 9.03 m/s^2 +Mag X: 32.78 uT Y: 22.70 uT Z: -58.77 uT +Accel X: -1.54 m/s^2 Y: 2.05 m/s^2 Z: 10.95 m/s^2 +Mag X: 36.95 uT Y: 27.70 uT Z: -56.99 uT +Accel X: -0.97 m/s^2 Y: -0.07 m/s^2 Z: 9.61 m/s^2 +Mag X: 38.92 uT Y: 31.13 uT Z: -55.28 uT +Accel X: -0.58 m/s^2 Y: -0.09 m/s^2 Z: 9.67 m/s^2 +Mag X: 40.44 uT Y: 32.86 uT Z: -54.88 uT +Accel X: -0.83 m/s^2 Y: 2.06 m/s^2 Z: 10.43 m/s^2 +Mag X: 40.24 uT Y: 29.87 uT Z: -56.07 uT +Accel X: -0.87 m/s^2 Y: 2.07 m/s^2 Z: 9.03 m/s^2 +Mag X: 39.75 uT Y: 21.86 uT Z: -55.82 uT +Accel X: -1.10 m/s^2 Y: 4.23 m/s^2 Z: 8.28 m/s^2 +Mag X: 38.23 uT Y: 13.39 uT Z: -55.35 uT +Accel X: -1.54 m/s^2 Y: 7.08 m/s^2 Z: 6.66 m/s^2 +Mag X: 36.99 uT Y: 5.41 uT Z: -52.91 uT +Accel X: -1.75 m/s^2 Y: 7.70 m/s^2 Z: 6.68 m/s^2 +Mag X: 36.17 uT Y: -2.03 uT Z: -49.37 uT +Accel X: -1.13 m/s^2 Y: 9.62 m/s^2 Z: 4.69 m/s^2 +Mag X: 35.38 uT Y: -9.11 uT Z: -43.85 uT +Accel X: -0.68 m/s^2 Y: 8.50 m/s^2 Z: 2.12 m/s^2 +Mag X: 36.29 uT Y: -14.44 uT Z: -36.85 uT +Accel X: -0.78 m/s^2 Y: 9.73 m/s^2 Z: 0.28 m/s^2 +Mag X: 36.93 uT Y: -17.80 uT Z: -29.96 uT +Accel X: -0.63 m/s^2 Y: 9.01 m/s^2 Z: -1.29 m/s^2 +Mag X: 38.26 uT Y: -20.33 uT Z: -20.36 uT +Accel X: 1.03 m/s^2 Y: 8.16 m/s^2 Z: -4.57 m/s^2 +Mag X: 41.19 uT Y: -20.18 uT Z: -10.68 uT +Accel X: 1.41 m/s^2 Y: 7.05 m/s^2 Z: -5.90 m/s^2 +Mag X: 42.75 uT Y: -18.65 uT Z: -4.36 uT +Accel X: 2.80 m/s^2 Y: 7.09 m/s^2 Z: -8.06 m/s^2 +Mag X: 47.38 uT Y: -14.07 uT Z: 3.89 uT +Accel X: 3.29 m/s^2 Y: 5.48 m/s^2 Z: -9.77 m/s^2 +Mag X: 49.78 uT Y: -8.96 uT Z: 8.89 uT +Accel X: 2.91 m/s^2 Y: 3.80 m/s^2 Z: -8.63 m/s^2 +Mag X: 49.85 uT Y: -5.58 uT Z: 12.23 uT +Accel X: 2.59 m/s^2 Y: 0.64 m/s^2 Z: -7.17 m/s^2 +Mag X: 49.99 uT Y: -1.39 uT Z: 15.62 uT +Accel X: 2.98 m/s^2 Y: 1.79 m/s^2 Z: -8.66 m/s^2 +Mag X: 51.61 uT Y: 2.24 uT Z: 17.82 uT +Accel X: 3.33 m/s^2 Y: 0.62 m/s^2 Z: -8.44 m/s^2 +Mag X: 52.51 uT Y: 4.44 uT Z: 17.49 uT +Accel X: 3.55 m/s^2 Y: -0.84 m/s^2 Z: -9.77 m/s^2 +Mag X: 55.57 uT Y: 9.05 uT Z: 17.87 uT +Accel X: 4.45 m/s^2 Y: -1.10 m/s^2 Z: -10.72 m/s^2 +Mag X: 56.85 uT Y: 17.52 uT Z: 18.15 uT +Accel X: 4.29 m/s^2 Y: -4.22 m/s^2 Z: -7.61 m/s^2 +Mag X: 57.21 uT Y: 26.56 uT Z: 17.82 uT +Accel X: 4.02 m/s^2 Y: -5.38 m/s^2 Z: -6.72 m/s^2 +Mag X: 57.99 uT Y: 33.91 uT Z: 16.16 uT +Accel X: 4.21 m/s^2 Y: -7.03 m/s^2 Z: -6.44 m/s^2 +Mag X: 58.48 uT Y: 39.59 uT Z: 13.56 uT +Accel X: 4.33 m/s^2 Y: -8.21 m/s^2 Z: -4.01 m/s^2 +Mag X: 58.29 uT Y: 44.65 uT Z: 10.16 uT +Accel X: 4.49 m/s^2 Y: -10.23 m/s^2 Z: -2.57 m/s^2 +Mag X: 60.44 uT Y: 47.16 uT Z: 4.00 uT +Accel X: 4.79 m/s^2 Y: -8.64 m/s^2 Z: -1.30 m/s^2 +Mag X: 60.84 uT Y: 51.48 uT Z: -1.49 uT +Accel X: 4.52 m/s^2 Y: -8.62 m/s^2 Z: 0.68 m/s^2 +Mag X: 61.06 uT Y: 53.13 uT Z: -8.49 uT +Accel X: 3.83 m/s^2 Y: -8.17 m/s^2 Z: 3.35 m/s^2 +Mag X: 61.53 uT Y: 53.14 uT Z: -15.20 uT +Accel X: 4.34 m/s^2 Y: -9.69 m/s^2 Z: 3.34 m/s^2 +Mag X: 61.82 uT Y: 51.67 uT Z: -22.92 uT +Accel X: 3.76 m/s^2 Y: -6.13 m/s^2 Z: 4.82 m/s^2 +Mag X: 59.98 uT Y: 50.95 uT Z: -29.54 uT +Accel X: 3.19 m/s^2 Y: -6.62 m/s^2 Z: 6.83 m/s^2 +Mag X: 58.73 uT Y: 48.80 uT Z: -34.04 uT +Accel X: 2.90 m/s^2 Y: -5.66 m/s^2 Z: 8.15 m/s^2 +Mag X: 57.34 uT Y: 45.86 uT Z: -38.97 uT +Accel X: 2.65 m/s^2 Y: -4.43 m/s^2 Z: 8.17 m/s^2 +Mag X: 55.54 uT Y: 43.57 uT Z: -43.31 uT +Accel X: 1.63 m/s^2 Y: -2.47 m/s^2 Z: 8.38 m/s^2 +Mag X: 52.85 uT Y: 40.11 uT Z: -47.21 uT +Accel X: 0.99 m/s^2 Y: -2.56 m/s^2 Z: 8.76 m/s^2 +Mag X: 50.96 uT Y: 37.36 uT Z: -49.52 uT +Accel X: 0.75 m/s^2 Y: -0.97 m/s^2 Z: 9.55 m/s^2 +Mag X: 49.06 uT Y: 34.80 uT Z: -50.53 uT +Accel X: 0.03 m/s^2 Y: 0.23 m/s^2 Z: 8.95 m/s^2 +Mag X: 46.74 uT Y: 31.80 uT Z: -52.75 uT +Accel X: -0.08 m/s^2 Y: -0.30 m/s^2 Z: 9.01 m/s^2 +Mag X: 44.67 uT Y: 33.05 uT Z: -52.85 uT +Accel X: -1.23 m/s^2 Y: -1.66 m/s^2 Z: 9.14 m/s^2 +Mag X: 41.00 uT Y: 37.08 uT Z: -53.33 uT +Accel X: -2.39 m/s^2 Y: -2.07 m/s^2 Z: 8.68 m/s^2 +Mag X: 35.09 uT Y: 42.14 uT Z: -53.08 uT +Accel X: -3.71 m/s^2 Y: -3.26 m/s^2 Z: 8.38 m/s^2 +Mag X: 29.36 uT Y: 45.81 uT Z: -51.94 uT +Accel X: -4.81 m/s^2 Y: -3.29 m/s^2 Z: 7.47 m/s^2 +Mag X: 24.79 uT Y: 48.33 uT Z: -50.35 uT +Accel X: -4.89 m/s^2 Y: -3.92 m/s^2 Z: 7.47 m/s^2 +Mag X: 21.56 uT Y: 50.15 uT Z: -49.56 uT +Accel X: -6.16 m/s^2 Y: -4.12 m/s^2 Z: 6.07 m/s^2 +Mag X: 18.44 uT Y: 50.67 uT Z: -47.85 uT +Accel X: -6.31 m/s^2 Y: -4.56 m/s^2 Z: 6.21 m/s^2 +Mag X: 16.53 uT Y: 52.18 uT Z: -46.43 uT +Accel X: -6.27 m/s^2 Y: -4.04 m/s^2 Z: 6.26 m/s^2 +Mag X: 15.68 uT Y: 50.99 uT Z: -46.99 uT +Accel X: -6.76 m/s^2 Y: -4.21 m/s^2 Z: 5.61 m/s^2 +Mag X: 15.54 uT Y: 50.16 uT Z: -47.15 uT +Accel X: -6.71 m/s^2 Y: -4.16 m/s^2 Z: 5.48 m/s^2 +Mag X: 14.70 uT Y: 49.40 uT Z: -47.46 uT +Accel X: -6.74 m/s^2 Y: -4.03 m/s^2 Z: 6.44 m/s^2 +Mag X: 15.52 uT Y: 47.92 uT Z: -48.11 uT +Accel X: -5.74 m/s^2 Y: -3.18 m/s^2 Z: 7.44 m/s^2 +Mag X: 17.98 uT Y: 42.44 uT Z: -52.46 uT +Accel X: -4.47 m/s^2 Y: 0.81 m/s^2 Z: 8.36 m/s^2 +Mag X: 22.87 uT Y: 28.41 uT Z: -56.84 uT +Accel X: -4.44 m/s^2 Y: 2.19 m/s^2 Z: 7.98 m/s^2 +Mag X: 26.07 uT Y: 19.47 uT Z: -57.00 uT +Accel X: -3.82 m/s^2 Y: 3.61 m/s^2 Z: 7.84 m/s^2 +Mag X: 29.00 uT Y: 9.62 uT Z: -54.24 uT +Accel X: -3.22 m/s^2 Y: 4.97 m/s^2 Z: 7.85 m/s^2 +Mag X: 33.00 uT Y: 1.75 uT Z: -51.18 uT +Accel X: -3.04 m/s^2 Y: 6.32 m/s^2 Z: 7.10 m/s^2 +Mag X: 34.33 uT Y: -3.24 uT Z: -48.14 uT +Accel X: -2.59 m/s^2 Y: 6.12 m/s^2 Z: 7.00 m/s^2 +Mag X: 37.09 uT Y: -3.70 uT Z: -47.49 uT +Accel X: -0.78 m/s^2 Y: 5.84 m/s^2 Z: 8.02 m/s^2 +Mag X: 42.82 uT Y: -2.06 uT Z: -47.40 uT +Accel X: 0.32 m/s^2 Y: 5.87 m/s^2 Z: 8.06 m/s^2 +Mag X: 47.87 uT Y: 0.50 uT Z: -46.70 uT +Accel X: 1.28 m/s^2 Y: 4.72 m/s^2 Z: 8.03 m/s^2 +Mag X: 51.20 uT Y: 4.02 uT Z: -46.77 uT +Accel X: 2.63 m/s^2 Y: 3.12 m/s^2 Z: 9.05 m/s^2 +Mag X: 55.36 uT Y: 10.89 uT Z: -47.44 uT +Accel X: 3.31 m/s^2 Y: 1.67 m/s^2 Z: 9.57 m/s^2 +Mag X: 59.38 uT Y: 17.95 uT Z: -44.77 uT +Accel X: 3.96 m/s^2 Y: 0.20 m/s^2 Z: 8.57 m/s^2 +Mag X: 61.87 uT Y: 23.95 uT Z: -42.62 uT +Accel X: 4.78 m/s^2 Y: -1.11 m/s^2 Z: 8.46 m/s^2 +Mag X: 63.87 uT Y: 30.49 uT Z: -39.30 uT +Accel X: 5.37 m/s^2 Y: -2.71 m/s^2 Z: 8.05 m/s^2 +Mag X: 64.53 uT Y: 35.68 uT Z: -35.97 uT +Accel X: 5.16 m/s^2 Y: -4.03 m/s^2 Z: 7.47 m/s^2 +Mag X: 63.80 uT Y: 42.66 uT Z: -32.46 uT +Accel X: 4.60 m/s^2 Y: -6.04 m/s^2 Z: 5.68 m/s^2 +Mag X: 61.78 uT Y: 49.11 uT Z: -26.43 uT +Accel X: 3.85 m/s^2 Y: -7.45 m/s^2 Z: 4.24 m/s^2 +Mag X: 58.93 uT Y: 54.88 uT Z: -20.33 uT +Accel X: 2.94 m/s^2 Y: -8.90 m/s^2 Z: 2.81 m/s^2 +Mag X: 57.85 uT Y: 56.91 uT Z: -15.95 uT +Accel X: 2.73 m/s^2 Y: -8.45 m/s^2 Z: 2.40 m/s^2 +Mag X: 56.14 uT Y: 58.20 uT Z: -13.93 uT +Accel X: 3.30 m/s^2 Y: -8.95 m/s^2 Z: 1.16 m/s^2 +Mag X: 56.40 uT Y: 58.62 uT Z: -11.87 uT +Accel X: 2.80 m/s^2 Y: -9.86 m/s^2 Z: 0.16 m/s^2 +Mag X: 56.99 uT Y: 57.26 uT Z: -7.69 uT +Accel X: 2.92 m/s^2 Y: -9.52 m/s^2 Z: -0.37 m/s^2 +Mag X: 55.80 uT Y: 57.82 uT Z: -6.42 uT +Accel X: 2.20 m/s^2 Y: -8.94 m/s^2 Z: -1.67 m/s^2 +Mag X: 54.19 uT Y: 57.73 uT Z: -1.24 uT +Accel X: 1.90 m/s^2 Y: -8.96 m/s^2 Z: -2.80 m/s^2 +Mag X: 52.47 uT Y: 56.20 uT Z: 5.42 uT +Accel X: 2.51 m/s^2 Y: -6.26 m/s^2 Z: -4.77 m/s^2 +Mag X: 53.60 uT Y: 43.92 uT Z: 16.41 uT +Accel X: 5.08 m/s^2 Y: -9.97 m/s^2 Z: -12.11 m/s^2 +Mag X: 61.59 uT Y: 23.44 uT Z: 15.73 uT +Accel X: 3.48 m/s^2 Y: -0.62 m/s^2 Z: -5.25 m/s^2 +Mag X: 55.61 uT Y: 28.15 uT Z: 21.18 uT +Accel X: 1.15 m/s^2 Y: -3.45 m/s^2 Z: -9.93 m/s^2 +Mag X: 54.03 uT Y: 33.29 uT Z: 20.53 uT +Accel X: 1.40 m/s^2 Y: -1.52 m/s^2 Z: -9.08 m/s^2 +Mag X: 53.64 uT Y: 33.89 uT Z: 20.70 uT +Accel X: 0.95 m/s^2 Y: -3.19 m/s^2 Z: -9.89 m/s^2 +Mag X: 54.18 uT Y: 29.09 uT Z: 20.72 uT +Accel X: 2.07 m/s^2 Y: -1.11 m/s^2 Z: -9.98 m/s^2 +Mag X: 53.52 uT Y: 25.62 uT Z: 22.71 uT +Accel X: 0.96 m/s^2 Y: -1.54 m/s^2 Z: -9.40 m/s^2 +Mag X: 52.50 uT Y: 22.98 uT Z: 23.76 uT +Accel X: 1.28 m/s^2 Y: -1.41 m/s^2 Z: -10.14 m/s^2 +Mag X: 52.48 uT Y: 19.57 uT Z: 22.81 uT +Accel X: 0.81 m/s^2 Y: 1.19 m/s^2 Z: -9.31 m/s^2 +Mag X: 51.86 uT Y: 13.46 uT Z: 22.30 uT +Accel X: 0.57 m/s^2 Y: 0.69 m/s^2 Z: -10.20 m/s^2 +Mag X: 51.64 uT Y: 10.99 uT Z: 22.04 uT +Accel X: 0.35 m/s^2 Y: 2.05 m/s^2 Z: -10.46 m/s^2 +Mag X: 51.96 uT Y: 9.38 uT Z: 21.54 uT +Accel X: 0.59 m/s^2 Y: 3.33 m/s^2 Z: -9.53 m/s^2 +Mag X: 50.77 uT Y: 6.21 uT Z: 20.42 uT +Accel X: 1.39 m/s^2 Y: 3.77 m/s^2 Z: -9.62 m/s^2 +Mag X: 52.28 uT Y: 4.28 uT Z: 19.18 uT +Accel X: 0.86 m/s^2 Y: 3.14 m/s^2 Z: -8.96 m/s^2 +Mag X: 52.48 uT Y: 5.79 uT Z: 19.26 uT +Accel X: 2.50 m/s^2 Y: 2.50 m/s^2 Z: -8.80 m/s^2 +Mag X: 53.33 uT Y: 9.18 uT Z: 20.46 uT +Accel X: 1.87 m/s^2 Y: 0.87 m/s^2 Z: -9.75 m/s^2 +Mag X: 54.75 uT Y: 14.38 uT Z: 21.32 uT +Accel X: 1.59 m/s^2 Y: -1.19 m/s^2 Z: -9.34 m/s^2 +Mag X: 53.79 uT Y: 24.39 uT Z: 22.67 uT +Accel X: 1.67 m/s^2 Y: -3.88 m/s^2 Z: -8.29 m/s^2 +Mag X: 52.70 uT Y: 35.50 uT Z: 21.03 uT +Accel X: 1.96 m/s^2 Y: -5.73 m/s^2 Z: -7.96 m/s^2 +Mag X: 54.47 uT Y: 41.51 uT Z: 16.35 uT +Accel X: 2.66 m/s^2 Y: -6.77 m/s^2 Z: -6.47 m/s^2 +Mag X: 55.12 uT Y: 47.57 uT Z: 11.22 uT +Measurements done. Please wait for the calibrator to setup! +Calibration parameters of the magnetometer: + +Average magnitude (default Hm) and sigma of 300 vectors = 65.016.5 +Hm is set to: 64.95 + +Correction matrix Ainv, using Hm=65.0: +1.43589 0.03716 -0.00720 +0.03716 1.44163 0.02165 +-0.00720 0.02165 1.49330 + +Where Hcalc = Ainv*(H-B) + +Code initialization statements... + +float B[3] = {28.69,23.06,-14.63}; + +float Ainv[3][3] = { + {1.43589,0.03716,-0.00720}, + {0.03716,1.44163,0.02165}, + {-0.00720,0.02165,1.49330} +}; + +RMS corrected vector length 0.234494 + +RMS residual 0.996390 +Calibration parameters of the accelerometer: + +Average magnitude (default Hm) and sigma of 300 vectors = 9.90.7 +Hm is set to: 9.86 + +Correction matrix Ainv, using Hm=9.9: +1.03170 0.01064 0.00913 +0.01064 1.00277 -0.01532 +0.00913 -0.01532 0.98588 + +Where Hcalc = Ainv*(H-B) + +Code initialization statements... + +float B[3] = {-0.14,-0.14,-0.19}; + +float Ainv[3][3] = { + {1.03170,0.01064,0.00913}, + {0.01064,1.00277,-0.01532}, + {0.00913,-0.01532,0.98588} +}; + +RMS corrected vector length 0.048460 + +RMS residual 0.995087 \ No newline at end of file