add some code

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/CMakeLists.txt

@@ -0,0 +1,8 @@
+# For more information about build system see
+# https://docs.espressif.com/projects/esp-idf/en/latest/api-guides/build-system.html
+# The following five lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+add_compile_options("-Wno-format")
+project(esp-dsp-azure-board-app-3d-graphics)

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/README.md

@@ -0,0 +1,27 @@
+# ESP-DSP ESP32-Azure IoT kit 3d graphics demo application
+
+The demo is developed for [ESP32-Azure IoT kit](https://github.com/espressif/esp-bsp/tree/master/esp32_azure_iot_kit) development board and is demonstrating the usage of matrices with `ESP-DSP` `Mat` class, Kalman filter and basic 3D graphics.
+
+The 3D Graphics demo displays a 2D graphics, converted to 3D as a 3D rotating object, on the development board's display. Button press changes the rotation direction of the 3D object. Run the menuconfig using the following command: 
+
+    idf.py mencuonfig
+
+In the menuconfig's menu item `Demo user configuration` select which 3D object to display. It's either a 3D cube, or ESP logo, or a user-defined graphics. Getting the user-defined graphics is described in an [example](../../graphics/img_to_3d_matrix/example/)
+
+## Running the demo
+
+To start the demo, run the following command:
+
+    idf.py build flash monitor
+
+The expected output is the following:
+
+    I (570) 3D image demo: Selected 3D image - ESP Logo
+    I (570) 3D image demo: Showing ESP text
+    I (6730) 3D image demo: Showing 3D image
+
+Note, that the first line `Selected 3D image` from the expected output depends on the user's Kconfing menu selection
+
+<div align="center">
+  <img src= "applications/azure_board_apps/apps/3d_graphics/3d_graphics.gif">
+</div>

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/main/3d_graphics_demo.cpp

@@ -0,0 +1,240 @@
+/*
+ * SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include "esp_log.h"
+#include "ssd1306.h"
+#include "bsp/esp-bsp.h"
+#include "esp_dsp.h"
+#include "cube_matrix.h"
+#include "esp_logo.h"
+#include "esp_text.h"
+#include "graphics_support.h"
+#include "image_to_3d_matrix.h"
+
+static ssd1306_handle_t ssd1306_dev = NULL;
+static bool button_pressed = true;
+
+dspm::Mat perspective_matrix(MATRIX_SIZE, MATRIX_SIZE);
+
+extern "C" void app_main();
+
+/**
+ * @brief Initialize 3d image structure
+ *
+ * Assigns a 3d image to be displayed to the 3d image structure based on the Kconfig menu result.
+ * The Kconfig menu is operated by a user
+ *
+ * @param image: 3d image structure
+ */
+static void init_3d_matrix_struct(image_3d_matrix_t *image)
+{
+#ifdef CONFIG_3D_OBJECT_ESP_LOGO
+    image->matrix = image_3d_matrix_esp_logo;
+    image->matrix_len = ((sizeof(image_3d_matrix_esp_logo)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("3D image demo", "Selected 3D image - ESP Logo");
+#elif CONFIG_3D_OBJECT_CUSTOM
+    image->matrix = image_to_3d_matrix_custom;
+    image->matrix_len = ((sizeof(image_to_3d_matrix_custom)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("3D image demo", "Selected 3D image - User's custom image");
+#elif CONFIG_3D_OBJECT_CUBE
+    image->matrix = cube_vectors_3d;
+    image->matrix_len = ((sizeof(cube_vectors_3d)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("3D image demo", "Selected 3D image - 3D cube");
+#endif
+}
+
+/**
+ * @brief Initialize display
+ */
+static void app_ssd1306_init(void)
+{
+    ssd1306_dev = ssd1306_create((i2c_port_t)BSP_I2C_NUM, SSD1306_I2C_ADDRESS);
+    ssd1306_clear_screen(ssd1306_dev, 0x00);
+    ssd1306_refresh_gram(ssd1306_dev);
+}
+
+/**
+ * @brief Display a 3d image
+ *
+ * If the object is the 3d cube, connect the projected cube points by lines and display the lines
+ * For any other 3d object lit pixels on the display from provided XY coordinates
+ *
+ * @param projected_image: 3d matrix from Mat class after projection
+ */
+static void display_3d_image(dspm::Mat projected_image)
+{
+    ssd1306_clear_screen(ssd1306_dev, 0);
+
+    if (OBJECT_3D_CUBE) {
+        // For the 3D cube, only the 6 points of the cube are transformed
+        // Cube edges, connecting transformed 3D cube points are connected with lines here
+        for (uint8_t cube_point = 0; cube_point < CUBE_EDGES; cube_point++) {
+            ssd1306_draw_line(ssd1306_dev,
+                              (int16_t)projected_image(cube_dict_line_begin[cube_point], 0),
+                              (int16_t)projected_image(cube_dict_line_begin[cube_point], 1),
+                              (int16_t)projected_image(cube_dict_line_end[cube_point], 0),
+                              (int16_t)projected_image(cube_dict_line_end[cube_point], 1));
+        }
+    } else {
+        // Every other 3D image is drawn here pixel by pixel
+        for (uint32_t pixel = 0; pixel < projected_image.rows; pixel++ ) {
+            ssd1306_fill_point(ssd1306_dev, projected_image(pixel, 0), projected_image(pixel, 1), 1);
+        }
+    }
+    ssd1306_refresh_gram(ssd1306_dev);
+}
+
+/**
+ * @brief Display ESPRESSIF text
+ *
+ * To demonstrate usage of the translation and scaling matrices
+ */
+static void dispaly_esp_text(void)
+{
+    image_3d_matrix_t esp_text;
+    esp_text.matrix = image_3d_array_esp_text;
+    esp_text.matrix_len = ((sizeof(image_3d_array_esp_text)) / sizeof(float)) / MATRIX_SIZE;
+    int16_t shift_x = -SSD1606_X_CENTER;
+
+    dspm::Mat T = dspm::Mat::eye(MATRIX_SIZE);                      // Transformation matrix
+    dspm::Mat transformed_image(esp_text.matrix_len, MATRIX_SIZE);  // 3D image matrix after transformation
+    dspm::Mat matrix_3d((float *)esp_text.matrix[0], esp_text.matrix_len, MATRIX_SIZE);
+
+    ESP_LOGI("3D image demo", "Showing ESP text");
+
+    for (int i = 0; i < 52; i++) {
+        update_translation_matrix(T, true, (float)shift_x, (float)SSD1606_Y_CENTER, 0);
+        transformed_image = matrix_3d * T;
+
+        ssd1306_clear_screen(ssd1306_dev, 0);
+        for (uint32_t point = 0; point < transformed_image.rows; point++ ) {
+            ssd1306_fill_point(ssd1306_dev, transformed_image(point, 0), transformed_image(point, 1), 1);
+        }
+        ssd1306_refresh_gram(ssd1306_dev);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+
+        shift_x += 5;
+    }
+
+    ssd1306_clear_screen(ssd1306_dev, 0);
+    ssd1306_draw_bitmap(ssd1306_dev, 0, 24, &image_bmp_array_esp_text[0], 128, 24);
+    ssd1306_refresh_gram(ssd1306_dev);
+
+    update_translation_matrix(T, true, (float)SSD1606_X_CENTER, (float)SSD1606_Y_CENTER, 0);
+    vTaskDelay(100 / portTICK_PERIOD_MS);
+
+    float scale = 1;
+    for (int i = 0; i < 20; i++) {
+        update_scaling_matrix(T, false, scale, scale, 1);
+        transformed_image = matrix_3d * T;
+
+        ssd1306_clear_screen(ssd1306_dev, 0);
+        for (uint32_t point = 0; point < transformed_image.rows; point++ ) {
+            ssd1306_fill_point(ssd1306_dev, transformed_image(point, 0), transformed_image(point, 1), 1);
+        }
+        ssd1306_refresh_gram(ssd1306_dev);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+
+        if (i < 10) {
+            scale -= 0.05;
+        } else {
+            scale += 0.05;
+        }
+    }
+}
+
+/**
+ * @brief RTOS task to draw a 3d image.
+ *
+ * Updates 3d matrices, prepares the final 3d matrix to be displayed on the display
+ *
+ * @param arg: pointer to RTOS task arguments, 3d image structure in this case
+ */
+static void draw_3d_image_task(void *arg)
+{
+    float rot_y = 0, rot_x = 0;
+    const float angle_increment = 4;
+    image_3d_matrix_t *image = (image_3d_matrix_t *)arg;
+
+    dspm::Mat T = dspm::Mat::eye(MATRIX_SIZE);                      // Transformation matrix
+    dspm::Mat transformed_image(image->matrix_len, MATRIX_SIZE);    // 3D image matrix after transformation
+    dspm::Mat projected_image(image->matrix_len, MATRIX_SIZE);      // 3D image matrix after projection
+    dspm::Mat matrix_3d((float *)image->matrix[0], image->matrix_len, MATRIX_SIZE);
+
+    if (OBJECT_3D_CUBE) {
+        rot_x = 45;
+    }
+
+    while (1) {
+        if (button_pressed) {
+            rot_y += angle_increment;
+            if (rot_y >= 360) {
+                rot_y -= 360;
+            }
+        } else {
+            rot_y -= angle_increment;
+            if (rot_y <= 0) {
+                rot_y += 360;
+            }
+        }
+
+        // Apply rotation in all the axes to the transformation matrix
+        update_rotation_matrix(T, rot_x, rot_y, 0);
+        // Apply translation to the transformation matrix
+        update_translation_matrix(T, true, ((float)SSD1606_X_CENTER), ((float)SSD1606_Y_CENTER), 0);
+
+        // explanation for the matrix multiplication is for the 3D cube scenario, applies for all of the objects
+        // where matrix rows for the transformed image and the projected image are set according to the specific 3d object
+
+        // matrix mul cube_matirx(8x4) * transformation_matrix(4x4) = transformed_cube(8x4)
+        transformed_image = matrix_3d * T;
+        // matrix mul transformed_cube(8x4) * perspective_matrix(4x4) = projected_cube(8x4)
+        projected_image = transformed_image * perspective_matrix;
+
+        display_3d_image(projected_image);
+        vTaskDelay(20 / portTICK_PERIOD_MS);
+    }
+}
+
+void app_main(void)
+{
+    static bool button_prev_val = false;
+    image_3d_matrix_t image;
+    ekf_imu13states *ekf13 = new ekf_imu13states();
+    ekf13->Init();
+
+    // Init all board components
+    bsp_i2c_init();
+    app_ssd1306_init();                         // display init
+    bsp_leds_init();                            // LEDs init
+    bsp_i2c_set_clk_speed(I2C_CLK_600KHZ);      // Set I2C to 600kHz
+
+    init_perspective_matrix(perspective_matrix);
+    init_3d_matrix_struct(&image);
+
+    dispaly_esp_text();
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
+
+    xTaskCreate(draw_3d_image_task, "draw_3d_image", 2048, &image, 4, NULL);
+    ESP_LOGI("3D image demo", "Showing 3D image");
+
+    while (1) {
+        if (bsp_button_get()) {
+            button_pressed = !button_pressed;
+        }
+
+        if (button_prev_val != button_pressed) {
+            button_prev_val = button_pressed;
+            if (button_pressed) {
+                bsp_led_set(BSP_LED_AZURE, true);
+            } else {
+                bsp_led_set(BSP_LED_AZURE, false);
+            }
+        }
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+    }
+}

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/main/CMakeLists.txt

@@ -0,0 +1,8 @@
+idf_component_register(SRCS "3d_graphics_demo.cpp" 
+                            "../../../graphics/3d_matrix/3d_matrix_data/esp_logo.c"
+                            "../../../graphics/3d_matrix/3d_matrix_data/esp_text.c" 
+                            "../../../graphics/3d_matrix/3d_matrix_data/image_to_3d_matrix.c"
+                            "../../../graphics/3d_matrix/3d_matrix_src/graphics_support.cpp" 
+                       INCLUDE_DIRS "."
+                                    "../../../graphics/3d_matrix/3d_matrix_data"
+                                    "../../../graphics/3d_matrix/3d_matrix_src")

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/main/Kconfig.projbuild

@@ -0,0 +1,19 @@
+menu "Demo user configuration"
+    choice
+        prompt "Select 3D object"
+        config 3D_OBJECT_CUBE
+            bool "3D cube"
+            help
+                3D graphics to be displayed is cube
+
+        config 3D_OBJECT_ESP_LOGO
+            bool "3D ESP Logo"
+            help
+                3D graphics to be displayed is ESP Logo
+
+        config 3D_OBJECT_CUSTOM
+            bool "User-defined graphics"
+            help
+                3D graphics to be displayed is a user-defined graphics
+    endchoice
+endmenu

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/3d_graphics/main/idf_component.yml

@@ -0,0 +1,8 @@
+## IDF Component Manager Manifest File
+description: ESP-DSP azure board application 3d graphics
+dependencies:
+  espressif/esp32_azure_iot_kit: "*"
+  espressif/esp-dsp:
+    version: "*"
+    override_path: "../../../../../../esp-dsp"
+

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/CMakeLists.txt

@@ -0,0 +1,8 @@
+# For more information about build system see
+# https://docs.espressif.com/projects/esp-idf/en/latest/api-guides/build-system.html
+# The following five lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+add_compile_options("-Wno-format")
+project(esp-dsp-azure-board-app-kalman-filter)

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/README.md

@@ -0,0 +1,59 @@
+# ESP-DSP ESP32-Azure IoT kit Kalman filter demo application
+
+The demo is developed for [ESP32-Azure IoT kit](https://github.com/espressif/esp-bsp/tree/master/esp32_azure_iot_kit) development board and is demonstrating the usage of matrices with `ESP-DSP` `Mat` class, Kalman filter and basic 3D graphics.
+
+The Kalman filter demo displays a 2D graphics, converted to 3D as a 3D object, on the development board's display. The 3D object follows the movements of the development board, where the Kalman filter is used for processing the output signals of the IMU sensors accommodated on the development board. The 3D object rotation is calculated by the Kalman filter class methods. All 3 IMU sensors present on the dev board (accelerometer, gyroscope and magnetometer) are used for sensing the development board's position.
+
+
+If the board is inactive (no, or very low rotation is detected) for a set period of time, the demo enters an "Idle" state, in which a 3D rotating object is displayed. Once a certain set level of the board's rotation is detected, the demo enters a normal, "Active", state.
+
+For the project settings, run the menuconfig using the following command: 
+
+    idf.py mencuonfig
+
+In the menuconfig's menu item `Demo user configuration` select which 3D object to display. It's either 3D cube, or ESP logo, or a user-defined graphics. Getting the user-defined 3D object is described in the [3D Graphics demo](../3d_graphics)
+
+## Kalman filter
+#### Calibration
+
+The filter must be calibrated before the first run, which takes several minutes. But the calibration process before each run can be omitted by calibrating the filter once, saving Kalman's filter state vectors to the NVS, and loading those vectors back into the Kalman filter before the run. In addition, every 5 minutes a current state vectors are saved into the flash memory.
+
+## Running the demo
+
+To start the demo, run the following command:
+
+    idf.py build flash monitor
+
+The expected output is the following:
+
+    I (589) Kalman filter demo: Selected 3D image - 3D cube
+    I (590) Kalman filter demo: Filter state vectors present in the NVS
+    I (592) Kalman filter demo: Loading state vectors into the filter structure
+    I (604) Kalman filter demo: State vectors loaded from the NVS
+    I (606) Barometer: disabled
+    I (619) Board status: board put to active mode
+    I (95780) Board status: board put to idle mode
+    I (300619) Kalman filter demo: State vectors saved to NVS
+
+Note, that the first line `Selected 3D image` from the expected output depends on the user's Kconfing menu selection
+
+To start the demo and run the initial Kalman filter calibration, one must erase the flash memory, to remove the previously stored Kalman filter's state vectors. To do so, run the following command:
+
+    idf.py erase_flash build flash monitor
+
+The expected output is the following:
+
+    I (592) Kalman filter demo: Selected 3D image - 3D cube
+    I (595) Kalman filter demo: Filter state vectors not present in the NVS
+    I (595) Kalman filter demo: Starting Kalman filter calibration loop
+    I (100699) Kalman filter demo: Exiting Kalman filter calibration loop
+    I (100894) Kalman filter demo: Estimated gyroscope bias error [deg/sec]: -0.020715	-0.000431	-0.022452
+    I (100900) Kalman filter demo: State vectors saved to the NVS
+    I (100900) Barometer: disabled
+    I (100911) Board status: board put to active mode
+    I (196072) Board status: board put to idle mode
+    I (400912) Kalman filter demo: State vectors saved to NVS
+
+<div align="center">
+  <img src= "applications/azure_board_apps/apps/kalman_filter/kalman_filter.gif">
+</div>

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/main/CMakeLists.txt

@@ -0,0 +1,7 @@
+idf_component_register(SRCS "kalman_filter_demo.cpp"
+                            "../../../graphics/3d_matrix/3d_matrix_data/esp_logo.c"
+                            "../../../graphics/3d_matrix/3d_matrix_data/image_to_3d_matrix.c"
+                            "../../../graphics/3d_matrix/3d_matrix_src/graphics_support.cpp" 
+                       INCLUDE_DIRS "."
+                                    "../../../graphics/3d_matrix/3d_matrix_data"
+                                    "../../../graphics/3d_matrix/3d_matrix_src")

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/main/Kconfig.projbuild

@@ -0,0 +1,19 @@
+menu "Demo user configuration"
+    choice
+        prompt "Select 3D object"
+        config 3D_OBJECT_CUBE
+            bool "3D cube"
+            help
+                3D graphics to be displayed is cube
+
+        config 3D_OBJECT_ESP_LOGO
+            bool "3D ESP Logo"
+            help
+                3D graphics to be displayed is ESP Logo
+
+        config 3D_OBJECT_CUSTOM
+            bool "User-defined graphics"
+            help
+                3D graphics to be displayed is a user-defined graphics
+    endchoice
+endmenu

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/main/idf_component.yml

@@ -0,0 +1,8 @@
+## IDF Component Manager Manifest File
+description: ESP-DSP azure board application Kalman filter
+dependencies:
+  espressif/esp32_azure_iot_kit: "*"
+  espressif/esp-dsp:
+    version: "*"
+    override_path: "../../../../../../esp-dsp"
+

managed_components/espressif__esp-dsp/examples/azure_board_apps/apps/kalman_filter/main/kalman_filter_demo.cpp

@@ -0,0 +1,608 @@
+/*
+ * SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <malloc.h>
+#include "mpu6050.h"
+#include "ssd1306.h"
+#include "mag3110.h"
+#include "fbm320.h"
+#include "bsp/esp-bsp.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "esp_idf_version.h" // for backward compatibility of esp-timer
+#include "nvs.h"
+#include "nvs_flash.h"
+#include "graphics_support.h"
+#include "cube_matrix.h"
+#include "esp_dsp.h"
+#include "ekf_imu13states.h"
+#include "esp_logo.h"
+#include "image_to_3d_matrix.h"
+
+#define STORAGE_NAMESPACE "kalman_filter"
+#define USE_BAROMETER 0
+
+static ssd1306_handle_t ssd1306_dev = NULL;
+static mpu6050_handle_t mpu6050_dev = NULL;
+static mag3110_handle_t mag3110_dev = NULL;
+static fbm320_handle_t fbm320_dev = NULL;
+
+static bool kalman_filter_calibrated = false;
+static bool board_inactive = true;
+
+dspm::Mat perspective_matrix(MATRIX_SIZE, MATRIX_SIZE);
+
+extern "C" void app_main();
+
+/**
+ * @brief Initialize magnetometer
+ */
+static void app_mag3110_init(void)
+{
+    esp_err_t ret;
+    mag3110_dev = mag3110_create((i2c_port_t)BSP_I2C_NUM);
+    mag3110_start_raw(mag3110_dev, MAG3110_DR_OS_80_16);
+    assert(ESP_OK == ret);
+}
+
+/**
+ * @brief Initialize display
+ */
+static void app_ssd1306_init(void)
+{
+    ssd1306_dev = ssd1306_create((i2c_port_t)BSP_I2C_NUM, SSD1306_I2C_ADDRESS);
+    ssd1306_clear_screen(ssd1306_dev, 0x00);
+    ssd1306_refresh_gram(ssd1306_dev);
+}
+
+/**
+ * @brief Initialize accelerometer and gyroscope
+ */
+static void mpu6050_init(void)
+{
+    esp_err_t ret;
+    mpu6050_dev = mpu6050_create((i2c_port_t)BSP_I2C_NUM, MPU6050_I2C_ADDRESS);
+    ret = mpu6050_config(mpu6050_dev, ACCE_FS_8G, GYRO_FS_2000DPS);
+    assert(ESP_OK == ret);
+    ret = mpu6050_wake_up(mpu6050_dev);
+    assert(ESP_OK == ret);
+}
+
+/**
+ * @brief Initialize pressure sensor
+ */
+static void app_fbm320_init(void)
+{
+    esp_err_t ret;
+    fbm320_dev = fbm320_create((i2c_port_t)BSP_I2C_NUM, FBM320_I2C_ADDRESS_1);
+    ret = fbm320_init(fbm320_dev);
+    assert(ESP_OK == ret);
+}
+
+/**
+ * @brief Initialize NVS flash memory
+ */
+static void init_nvs_flash_memory(void)
+{
+    esp_err_t err = nvs_flash_init();
+    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+        // NVS partition was truncated and needs to be erased
+        // Retry nvs_flash_init
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        err = nvs_flash_init();
+    }
+    ESP_ERROR_CHECK( err );
+}
+
+/**
+ * @brief Initialize 3d image structure
+ *
+ * Assigns a 3d image to be displayed to the 3d image structure based on the Kconfig menu result.
+ * The Kconfig menu is operated by a user
+ *
+ * @param image: pointer to 3d image structure
+ * @param ekf13: kalman filter object
+ */
+static void init_3d_matrix_struct(image_3d_matrix_kalman_t *image, ekf_imu13states *ekf13)
+{
+#ifdef CONFIG_3D_OBJECT_ESP_LOGO
+    image->matrix = image_3d_matrix_esp_logo;
+    image->matrix_len = ((sizeof(image_3d_matrix_esp_logo)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("Kalman filter demo", "Selected 3D image - ESP Logo");
+#elif CONFIG_3D_OBJECT_CUSTOM
+    image->matrix = image_to_3d_matrix_custom;
+    image->matrix_len = ((sizeof(image_to_3d_matrix_custom)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("Kalman filter demo", "Selected 3D image - User's custom image");
+#elif CONFIG_3D_OBJECT_CUBE
+    image->matrix = cube_vectors_3d;
+    image->matrix_len = ((sizeof(cube_vectors_3d)) / sizeof(float)) / MATRIX_SIZE;
+    ESP_LOGI("Kalman filter demo", "Selected 3D image - 3D cube");
+#endif
+    image->ekf13 = ekf13;
+}
+
+/**
+ * @brief Display a 3d image
+ *
+ * If the object is the 3d cube, connect the projected cube points by lines and display the lines
+ * For any other 3d object lit pixels on the display from provided XY coordinates
+ *
+ * @param projected_image: 3d matrix from Mat class after projection
+ */
+static void display_3d_image(dspm::Mat projected_image)
+{
+    ssd1306_clear_screen(ssd1306_dev, 0);
+
+    if (OBJECT_3D_CUBE) {
+        // For the 3D cube, only the 6 points of the cube are transformed
+        // Cube edges, connecting transformed 3D cube points are connected with lines here
+        for (uint8_t cube_point = 0; cube_point < CUBE_EDGES; cube_point++) {
+            ssd1306_draw_line(ssd1306_dev,
+                              (int16_t)projected_image(cube_dict_line_begin[cube_point], 0),
+                              (int16_t)projected_image(cube_dict_line_begin[cube_point], 1),
+                              (int16_t)projected_image(cube_dict_line_end[cube_point], 0),
+                              (int16_t)projected_image(cube_dict_line_end[cube_point], 1));
+        }
+    } else {
+        // Every other 3D image is drawn here pixel by pixel
+        for (uint32_t pixel = 0; pixel < projected_image.rows; pixel++ ) {
+            ssd1306_fill_point(ssd1306_dev, projected_image(pixel, 0), projected_image(pixel, 1), 1);
+        }
+    }
+    ssd1306_refresh_gram(ssd1306_dev);
+}
+
+/**
+ * @brief Draw a 3d image
+ *
+ * Updates 3d matrices and prepares the final 3d matrix to be displayed on the display.
+ * Board inactivity check - decides which board mode to display (active or inactive), based on the board movements.
+ *
+ * @param ekf13: kalman filter object
+ * @param transformed_image: 3d matrix holding a 3d image after transformation
+ * @param projected_image: 3d matrix holding a 3d image after projection
+ * @param matrix_3d: 3d matrix holding the original 3d image, without any transformation
+ */
+static void draw_3d_image(ekf_imu13states *ekf13, dspm::Mat &transformed_image, dspm::Mat &projected_image, dspm::Mat &matrix_3d)
+{
+    static const float movement_treshold = 0.0001;      // threshold to decide between Idle and Active state of the board
+    static float inactive_rotation = 0;                 // rotation angle (in degrees) for Idle state
+    static unsigned int inactivity_count = 0;
+    static const unsigned int inactivity_count_treshold = 75;
+    static unsigned int inactivity_check = 0;           // activity of the board is being checked once in N calls of the function
+    static float prev_state_arr[3] = {0, 0, 0};         // holds the previous state of the Euler angles, to compare the diff
+
+    dspm::Mat T = dspm::Mat::eye(MATRIX_SIZE);          // Transformation matrix
+    dspm::Mat R1 = ekf::quat2rotm(ekf13->X.data);       // matrix(3x1) that holds x, y, z rotation data
+    dspm::Mat eul_angles = ekf::rotm2eul(R1);
+
+    // check if the board is active or not every N calls of the function
+    if (!(inactivity_check++ % 10)) {
+        dspm::Mat prev_state_mat(prev_state_arr, 3, 1);
+        dspm::Mat diff = eul_angles - prev_state_mat;
+        prev_state_mat = eul_angles;
+
+        float max_diff = fabs(diff(0, 0) * diff(1, 0) * diff(2, 0));
+
+        // wake-up the board if the current board movement crosses the threshold
+        if (board_inactive && (max_diff > movement_treshold)) {
+            board_inactive = false;
+            ESP_LOGI("Board status", "board put to active mode");
+        }
+
+        // if the board is awake, and the current movement of the board is lower than the threshold - the board is
+        // being moved with - run the inactivity_counter
+        // after some time (if the movement of the board has been lower than the threshold) put the board to idle mode
+        else if (!board_inactive && (max_diff < movement_treshold)) {
+            if (inactivity_count > inactivity_count_treshold) {
+                board_inactive = true;
+                inactivity_count = 0;
+                ESP_LOGI("Board status", "board put to idle mode");
+                update_perspective_matrix(perspective_matrix, 90);
+            }
+            inactivity_count++;
+        }
+
+        // if the board is awake and the current movement of the board is higher than the threshold clear the inactivity_counter
+        else if (!board_inactive && (max_diff >= movement_treshold)) {
+            inactivity_count = 0;
+        }
+    }
+
+    if (board_inactive) {
+        // board idle state - display a rotating cube
+        update_rotation_matrix(T, inactive_rotation += 3.0, 10.0, 10.0);
+    } else {
+        // board active state - 3D object follows movements of the board
+        eul_angles(2, 0) = -eul_angles(2, 0);
+        dspm::Mat R = ekf::eul2rotm(eul_angles.data);
+
+        // Enlarge rotation matrix from 3x3 to 4x4
+        // Copy rotation matrix R(3x3) to transformation matrix T_m(4x4)
+        for (int row = 0; row < R.rows; row++) {
+            for (int col = 0; col < R.cols; col++) {
+                T(row, col) = R(row, col);
+            }
+        }
+    }
+
+    // explanation for the matrix multiplication is for the 3D cube scenario, applies for all of the objects
+    // where matrix rows for the transformed image and the projected image are set according to the specific 3d object
+
+    // matrix mul cube_matirx(8x4) * transformation_matrix(4x4) = transformed_cube(8x4)
+    transformed_image = matrix_3d * T;
+    // matrix mul transformed_cube(8x4) * perspective_matrix(4x4) = projected_cube(8x4)
+    projected_image = transformed_image * perspective_matrix;
+    display_3d_image(projected_image);
+}
+
+/**
+ * @brief Kalman filter RTOS task (or ESP timer callback)
+ *
+ * Takes IMU sensors measurements to be processed by the Kalman filter
+ * Function is used as:
+ *      RTOS task - during normal Kalman filter operation
+ *      ESP Timer callback function - during Kalman filter calibration process
+ *
+ * @param arg: pointer to RTOS task arguments, 3d image structure in this case
+ */
+static void kalman_filter_task(void *arg)
+{
+    mpu6050_acce_value_t acce_sample;
+    mpu6050_gyro_value_t gyro_sample;
+    mag3110_result_t mag_sample;
+
+    image_3d_matrix_kalman_t *kalman_filter_args = (image_3d_matrix_kalman_t *)arg;
+    ekf_imu13states *ekf13 = kalman_filter_args->ekf13;
+    dspm::Mat transformed_image(kalman_filter_args->matrix_len, MATRIX_SIZE);    // 3D image matrix after transformation
+    dspm::Mat projected_image(kalman_filter_args->matrix_len, MATRIX_SIZE);      // 3D image matrix after projection
+    dspm::Mat matrix_3d((float *)kalman_filter_args->matrix[0], kalman_filter_args->matrix_len, MATRIX_SIZE);
+
+    // Covariance matrix for Kalman filter, set specifically for this development board IMU sensors
+    float R_m[10] = {0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.000001, 0.000001, 0.000001, 0.000001};
+    update_perspective_matrix(perspective_matrix, 90);
+
+    while (1) {
+        // dt calculation
+        static float prev_time = 0;
+        const float current_time = dsp_get_cpu_cycle_count();
+        float dt = 0;
+
+        // Crystal count difference conversion to Dt time constant
+        if (current_time > prev_time) {
+            dt = current_time - prev_time;
+            dt = dt / 240000000.0;
+        }
+        prev_time = current_time;
+
+        // Get all the sensors values
+        mpu6050_get_acce(mpu6050_dev, &acce_sample);
+        mpu6050_get_gyro(mpu6050_dev, &gyro_sample);
+        mag3110_get_magnetic_induction(mag3110_dev, &mag_sample);
+
+        // Make arrays from the sensors values
+        float gyro_input_arr[3] =  {gyro_sample.gyro_x,  gyro_sample.gyro_y,  gyro_sample.gyro_z};
+        float accel_input_arr[3] = {acce_sample.acce_x,  acce_sample.acce_y,  acce_sample.acce_z};
+        float mag_input_arr[3] =   {(float)mag_sample.x, (float)mag_sample.y, (float)mag_sample.z};
+
+        // Accel and Mag data to Mat class
+        dspm::Mat gyro_input_mat(gyro_input_arr, 3, 1);
+        dspm::Mat accel_input_mat(accel_input_arr, 3, 1);
+        dspm::Mat mag_input_mat(mag_input_arr, 3, 1);
+
+        // Normalize vectors
+        dspm::Mat accel_norm = accel_input_mat / accel_input_mat.norm();
+        dspm::Mat magn_norm = mag_input_mat / mag_input_mat.norm();
+        gyro_input_mat *= DEG_TO_RAD;
+
+        ekf13->Process(gyro_input_mat.data, dt);
+        ekf13->UpdateRefMeasurementMagn(accel_norm.data, magn_norm.data, R_m);
+
+        if (kalman_filter_calibrated) {
+            // Use the function as RTOS task for the filter calculation
+            draw_3d_image(ekf13, transformed_image, projected_image, matrix_3d);
+            vTaskDelay(20 / portTICK_PERIOD_MS);
+        } else {
+            // Use the function as a callback for kalman_filter_calibration_timer
+            break;
+        }
+    }
+}
+
+/**
+ * @brief Kalman filter calibration procedure
+ *
+ * The Kalman filter must be calibrated before the very first run. The state of the Kalman filter is saved
+ * into NVS after the calibration.
+ * The calibration is run, only if no Kalman filter state is saved in the NVS. Which occurs after erasing
+ * the flash memory. Power cycling the board does not remove the Kalman filter state from the NVS.
+ *
+ * @param image: pointer to 3d image structure
+ */
+static void kalman_filter_calibration(image_3d_matrix_kalman_t *image)
+{
+    ekf_imu13states *ekf13 = image->ekf13;
+    esp_err_t ret;
+    nvs_handle_t nvs_handle_kalman;
+    size_t state_vectors_size = 13 * 14;
+    float *state_vectors = (float *)malloc(state_vectors_size * sizeof(float));
+
+    ret = nvs_open(STORAGE_NAMESPACE, NVS_READWRITE, &nvs_handle_kalman);
+    if (ret != ESP_OK) {
+        ESP_LOGE("NVS error", "(%s) opening NVS!\n", esp_err_to_name(ret));
+        assert(ESP_OK == ret);
+    }
+
+    // Read previously saved blob, if available
+    size_t required_size = 0;  // value will default to 0, if not set yet in NVS
+    ret = nvs_get_blob(nvs_handle_kalman, "state_vectors", NULL, &required_size);
+    if (ret != ESP_OK && ret != ESP_ERR_NVS_NOT_FOUND) {
+        ESP_LOGE("NVS error", "(%s) reading data from NVS!\n", esp_err_to_name(ret));
+        assert(ESP_OK == ret);
+    }
+
+    if (required_size > 0) {
+        ESP_LOGI("Kalman filter demo", "Filter state vectors present in the NVS");
+        ESP_LOGI("Kalman filter demo", "Loading state vectors into the filter structure");
+
+        size_t state_vectors_size_addr = state_vectors_size * sizeof(float);
+        ret = nvs_get_blob(nvs_handle_kalman, "state_vectors", state_vectors, &state_vectors_size_addr);
+        if (ret != ESP_OK) {
+            ESP_LOGE("NVS error", "(%s) reading data from NVS!\n", esp_err_to_name(ret));
+            assert(ESP_OK == ret);
+        }
+
+        for (int i = 0; i < state_vectors_size; i++) {
+            if (i < state_vectors_size - 13) {
+                ekf13->P.data[i] = state_vectors[i];
+            } else {
+                ekf13->X.data[i - (state_vectors_size - 13)] = state_vectors[i];
+            }
+        }
+
+        ESP_LOGI("Kalman filter demo", "State vectors loaded from the NVS");
+        nvs_close(nvs_handle_kalman);
+
+    } else {
+        ESP_LOGI("Kalman filter demo", "Filter state vectors not present in the NVS");
+        const float kalman_timer_period_us = 100000;
+
+        // ESP timer for the Kalman filter calibration
+        const esp_timer_create_args_t kalman_calibration_timer_config = {
+            .callback = kalman_filter_task,
+            .arg = image,
+            .dispatch_method = ESP_TIMER_TASK,
+            .name = "kalman_filter_calibration_timer",
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(4, 3, 0)
+            .skip_unhandled_events = true,
+#endif
+        };
+
+        esp_timer_handle_t kalman_calibration_timer = NULL;
+        ret = esp_timer_create(&kalman_calibration_timer_config, &kalman_calibration_timer);
+        assert(ESP_OK == ret);
+        ssd1306_clear_screen(ssd1306_dev, 0x00);
+
+        ESP_LOGI("Kalman filter demo", "Starting Kalman filter calibration loop");
+
+        ssd1306_clear_screen(ssd1306_dev, 0x00);
+        ssd1306_draw_string(ssd1306_dev, 0, 16, (const uint8_t *)"Kalman filter", 16, 1);
+        ssd1306_draw_string(ssd1306_dev, 0, 32, (const uint8_t *)"calibration", 16, 1);
+        ssd1306_refresh_gram(ssd1306_dev);
+
+        ret = esp_timer_start_periodic(kalman_calibration_timer, kalman_timer_period_us);
+        assert(ESP_OK == ret);
+        vTaskDelay(100000 / portTICK_PERIOD_MS);
+
+        ret = esp_timer_stop(kalman_calibration_timer);
+        assert(ESP_OK == ret);
+        ret = esp_timer_delete(kalman_calibration_timer);
+        assert(ESP_OK == ret);
+        ESP_LOGI("Kalman filter demo", "Exiting Kalman filter calibration loop");
+        ssd1306_draw_string(ssd1306_dev, 0, 48, (const uint8_t *)"Done!", 16, 1);
+        ssd1306_refresh_gram(ssd1306_dev);
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+
+        dspm::Mat estimated_error(&ekf13->X.data[4], 3, 1);
+
+        ESP_LOGI("Kalman filter demo", "Estimated gyroscope bias error [deg/sec]: %.6f\t%.6f\t%.6f",
+                 estimated_error.data[0], estimated_error.data[1], estimated_error.data[2]);
+
+        for (int i = 0; i < state_vectors_size; i++) {
+            if (i < state_vectors_size - 13) {
+                state_vectors[i] = ekf13->P.data[i];
+            } else {
+                state_vectors[i] = ekf13->X.data[i - (state_vectors_size - 13)];
+            }
+        }
+
+        ret = nvs_set_blob(nvs_handle_kalman, "state_vectors", state_vectors, state_vectors_size * sizeof(float));
+        assert(ESP_OK == ret);
+        ret = nvs_commit(nvs_handle_kalman);
+        assert(ESP_OK == ret);
+        nvs_close(nvs_handle_kalman);
+        ESP_LOGI("Kalman filter demo", "State vectors saved to the NVS");
+    }
+    // Set the initial state of the X vector
+    ekf13->X(0, 0) = 1;
+    ekf13->X(0, 1) = 0;
+    ekf13->X(0, 2) = 0;
+    ekf13->X(0, 3) = 0;
+    free(state_vectors);
+    kalman_filter_calibrated = true;
+}
+
+/**
+ * @brief RTOS task to periodically save the filter state
+ *
+ * The Kalman filter state is periodically saved into the NVS, each 5 min. So a recent Kalman filter state
+ * could be loaded into the Kalman filter object after a power cycle.
+ *
+ * @param arg: pointer to RTOS task arguments, Kalman filter object in this case
+ */
+static void save_state_vectors_task(void *arg)
+{
+    esp_err_t ret;
+    size_t state_vectors_size = 13 * 14;
+    nvs_handle_t nvs_handle_kalman;
+    ekf_imu13states *ekf13 = (ekf_imu13states *)arg;
+    vTaskDelay((60000 * 5) / portTICK_PERIOD_MS);    //  5min
+
+    while (1) {
+        float *state_vectors = (float *)malloc(state_vectors_size * sizeof(float));
+
+        ret = nvs_open(STORAGE_NAMESPACE, NVS_READWRITE, &nvs_handle_kalman);
+        if (ret != ESP_OK) {
+            ESP_LOGE("NVS error", "(%s) opening NVS!\n", esp_err_to_name(ret));
+            assert(ESP_OK == ret);
+        }
+
+        for (int i = 0; i < state_vectors_size; i++) {
+            if (i < state_vectors_size - 13) {
+                state_vectors[i] = ekf13->P.data[i];
+            } else {
+                state_vectors[i] = ekf13->X.data[i - (state_vectors_size - 13)];
+            }
+        }
+
+        ret = nvs_set_blob(nvs_handle_kalman, "state_vectors", state_vectors, state_vectors_size * sizeof(float));
+        if (ret != ESP_OK && ret != ESP_ERR_NVS_NOT_FOUND) {
+            ESP_LOGE("NVS error", "(%s) writing data to NVS!\n", esp_err_to_name(ret));
+            assert(ESP_OK == ret);
+        }
+
+        ret = nvs_commit(nvs_handle_kalman);
+        if (ret != ESP_OK && ret != ESP_ERR_NVS_NOT_FOUND) {
+            ESP_LOGE("NVS error", "(%s) commiting data to NVS!\n", esp_err_to_name(ret));
+            assert(ESP_OK == ret);
+        }
+        nvs_close(nvs_handle_kalman);
+        ESP_LOGI("Kalman filter demo", "State vectors saved to NVS");
+        free(state_vectors);
+        vTaskDelay((60000 * 5) / portTICK_PERIOD_MS);    //  Save the State vectors each 5 min
+    }
+}
+
+/**
+ * @brief ROTS task to read a pressure
+ *
+ * Pressure is measured periodically to better average out a stable value of pressure
+ *
+ * @param arg: pointer to RTOS task arguments, pointer to the pressure variable in this case
+ */
+static void get_pressure_task(void *arg)
+{
+    int32_t real_p, real_t;
+    float *pressure_ptr = (float *)arg;
+    float pressure_global = *pressure_ptr;
+
+    int call_count = 0;
+    int call_count1 = 0;
+    float pressure_initial = pressure_global;
+    float last_pressure = pressure_global;
+    float baro_image = pressure_global;
+    bool changed = false;
+
+    while (1) {
+        if (ESP_OK == fbm320_get_data(fbm320_dev, FBM320_MEAS_PRESS_OSR_4096, &real_t, &real_p)) {
+            pressure_global = (0.999 * pressure_global) + (0.001 * ((float)real_p) / 1000.0);
+            call_count1++;
+        }
+
+        if (board_inactive) {
+            pressure_initial = pressure_global;
+            last_pressure = pressure_global;
+            baro_image = pressure_global;
+        } else {
+            call_count++;
+            if (fabs(pressure_global - last_pressure) > 0.0005) {        // cahnge more than 0.5 Pa
+                last_pressure = pressure_global;
+                baro_image = (0.9 * baro_image) + (0.1 * last_pressure);
+                changed = true;
+            }
+
+            if ((!(call_count % 10)) && changed) {
+                float fov = 90 + (1000.0 * ((float)(pressure_initial - baro_image)));
+                update_perspective_matrix(perspective_matrix, fov);
+                changed = false;
+            }
+        }
+        vTaskDelay(100 / portTICK_PERIOD_MS);
+    }
+}
+
+/**
+ * @brief read the initial value of pressure
+ */
+float get_initial_pressure(void)
+{
+    float pressure;
+    int32_t real_p, real_t;
+    int averagning_loop_count = 500;
+
+    ESP_LOGI("Barometer", "Averagining initial barometer pressure");
+    ssd1306_clear_screen(ssd1306_dev, 0x00);
+    ssd1306_draw_string(ssd1306_dev, 0, 16, (const uint8_t *)"Barometer", 16, 1);
+    ssd1306_draw_string(ssd1306_dev, 0, 32, (const uint8_t *)"averaging", 16, 1);
+    ssd1306_refresh_gram(ssd1306_dev);
+
+    // take the first pressure measurement
+    while (1) {
+        if (ESP_OK == fbm320_get_data(fbm320_dev, FBM320_MEAS_PRESS_OSR_4096, &real_t, &real_p)) {
+            pressure = ((float)real_p) / 1000.0;        // pressure in kPa
+            break;
+        }
+    }
+
+    // average first N measurements
+    while (averagning_loop_count--) {
+        if (ESP_OK == fbm320_get_data(fbm320_dev, FBM320_MEAS_PRESS_OSR_4096, &real_t, &real_p)) {
+            pressure = (0.999 * pressure) + (0.001 * ((float)real_p) / 1000.0);
+            vTaskDelay(100 / portTICK_PERIOD_MS);
+        }
+    }
+
+    ESP_LOGI("Barometer", "Initial value set");
+    ssd1306_draw_string(ssd1306_dev, 0, 48, (const uint8_t *)"Done!", 16, 1);
+    ssd1306_refresh_gram(ssd1306_dev);
+    return (pressure);
+}
+
+void app_main(void)
+{
+    image_3d_matrix_kalman_t image;
+    ekf_imu13states *ekf13 = new ekf_imu13states();
+    ekf13->Init();
+
+    // Init all board components
+    bsp_i2c_init();
+    app_ssd1306_init();         // display init
+    mpu6050_init();             // gyro, acc init
+    app_mag3110_init();         // magnetometer init
+    app_fbm320_init();          // barometer init
+    bsp_leds_init();            // LEDs init
+    init_nvs_flash_memory();    // Non-Volatile Storage
+
+    init_perspective_matrix(perspective_matrix);
+    init_3d_matrix_struct(&image, ekf13);
+    kalman_filter_calibration(&image);
+
+    // Use a barometer for measuring the altitude
+    if (USE_BAROMETER) {
+        float init_pressure = get_initial_pressure();
+        xTaskCreate(get_pressure_task, "get_pressure_task", 2048 * 4, &init_pressure, 4, NULL);
+    } else {
+        ESP_LOGI("Barometer", "disabled");
+    }
+
+    xTaskCreate(kalman_filter_task, "kalman_filter_task", 2048 * 4, &image, 5, NULL);
+    xTaskCreate(save_state_vectors_task, "save_state_vectors", 2048, ekf13, 6, NULL);
+
+    while (1) {
+        vTaskDelay(10000 / portTICK_PERIOD_MS);
+    }
+}