diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h
index 50476241a1..aeb5d11b56 100644
--- a/FluidNC/src/Kinematics/Cartesian.h
+++ b/FluidNC/src/Kinematics/Cartesian.h
@@ -7,6 +7,7 @@
 	Cartesian.h
 
 	This is a kinematic system for where the motors operate in the cartesian space.
+    All logical axis of the system perfectly alligned with physical axis of machine.
 */
 
 #include "Kinematics.h"
diff --git a/FluidNC/src/Kinematics/GenericCartesian.cpp b/FluidNC/src/Kinematics/GenericCartesian.cpp
new file mode 100644
index 0000000000..fc71f2dc61
--- /dev/null
+++ b/FluidNC/src/Kinematics/GenericCartesian.cpp
@@ -0,0 +1,188 @@
+// Copyright (c) 2020 -	Bart Dring
+// Copyright (c) 2023 -	Vlad A.
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#include "GenericCartesian.h"
+
+#include "src/Machine/MachineConfig.h"
+#include "src/Machine/Axes.h"  // ambiguousLimit()
+//#include "Skew.h"              // Skew, SkewAxis
+#include "src/Limits.h"
+
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+namespace Kinematics {
+    void GenericCartesian::init() {
+        log_info("Kinematic system: " << name());
+        init_position();
+    }
+
+    // Initialize the machine position
+    void GenericCartesian::init_position() {
+        auto n_axis = config->_axes->_numberAxis;
+        for (size_t axis = 0; axis < n_axis; axis++) {
+            set_motor_steps(axis, 0);  // Set to zeros
+        }
+    }
+
+    bool GenericCartesian::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
+        if (_mtx) {
+            _mtx->transform(_buffer, target);
+            return mc_move_motors(_buffer, pl_data);
+        }
+
+        // Without skew correction motor space is the same cartesian space, so we do no transform.
+        return mc_move_motors(target, pl_data);
+    }
+
+    void GenericCartesian::motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
+        if (_rev) {
+            _rev->transform(cartesian, motors);
+        } else {
+            // Without skew correction motor space is the same cartesian space, so we do no transform.
+            copyAxes(cartesian, motors);
+        }
+    }
+
+    void GenericCartesian::transform_cartesian_to_motors(float* motors, float* cartesian) {
+        // Without skew correction motor space is the same cartesian space, so we do no transform.
+        if (_mtx) {
+            _mtx->transform(motors, cartesian);
+        } else {
+            copyAxes(motors, cartesian);
+        }
+    }
+
+    bool GenericCartesian::canHome(AxisMask axisMask) {
+        if (ambiguousLimit()) {
+            log_error("Ambiguous limit switch touching. Manually clear all switches");
+            return false;
+        }
+        return true;
+    }
+
+    bool GenericCartesian::limitReached(AxisMask& axisMask, MotorMask& motorMask, MotorMask limited) {
+        // For Cartesian, the limit switches are associated with individual motors, since
+        // an axis can have dual motors each with its own limit switch.  We clear the motors in
+        // the mask whose limits have been reached.
+        clear_bits(motorMask, limited);
+
+        auto oldAxisMask = axisMask;
+
+        // Set axisMask according to the motors that are still running.
+        axisMask = Machine::Axes::motors_to_axes(motorMask);
+
+        // Return true when an axis drops out of the mask, causing replan
+        // on any remaining axes.
+        return axisMask != oldAxisMask;
+    }
+
+    void GenericCartesian::releaseMotors(AxisMask axisMask, MotorMask motors) {
+        auto axes   = config->_axes;
+        auto n_axis = axes->_numberAxis;
+        for (int axis = 0; axis < n_axis; axis++) {
+            if (bitnum_is_true(axisMask, axis)) {
+                auto paxis = axes->_axis[axis];
+                if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 0))) {
+                    paxis->_motors[0]->unlimit();
+                }
+                if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 1))) {
+                    paxis->_motors[1]->unlimit();
+                }
+            }
+        }
+    }
+
+    GenericCartesian::~GenericCartesian() {
+        if (_mtx) {
+            delete _mtx;
+        }
+
+        if (_rev) {
+            delete _rev;
+        }
+    }
+
+    ////////////////////
+
+    template <typename number>
+    void GenericCartesian::Mtx<number>::dumpRow(const size_t rowIdx) const {
+        // Prints a row of the matrix into log.
+        // Useful for debuging.
+        std::ostringstream msg;
+        for (size_t col = 0; col < _pitch; ++col) {
+            number V = value( rowIdx, col );
+            msg << std::fixed << std::setw(9) << std::setprecision(5) << V;
+        }
+
+        log_debug( msg.str() );
+    }
+
+    template <typename number>
+    void GenericCartesian::Mtx<number>::dump() const {
+        for (size_t i = 0; i < _lines; ++i) {
+            dumpRow(i);
+        }
+    }
+
+    template <typename number>
+    void GenericCartesian::Mtx<number>::transform(number* to, const number* from) const {
+        for (size_t j = 0; j < _pitch; ++j) {
+            number A = 0.0;
+            for (size_t i = 0; i < _lines; ++i) {
+                A += from[i] * value(i, j);
+            }
+
+            to[j] = A;
+        }
+    }
+
+    bool GenericCartesian::GJ_invertMatrix(const size_t size, const Mtx<float>* A, Mtx<float>* const B) {
+        //log_debug( "GJ_invertMatrix" );
+        // Gauss Jordan Matrix inversion.
+        Mtx<double> T(size, size * 2);
+        size_t      i, j, k;
+
+        T.allocate();
+
+        for (i = 0; i < size; ++i) {
+            for (j = 0; j < size; ++j) {
+                *T.ptr(i, j)        = A->value(i, j);
+                *T.ptr(i, j + size) = (i == j) ? 1.0 : 0.0;
+            }
+        }
+
+        //T.dump();
+        for (i = 0; i < size; ++i) {
+            if (T.value(i, i) == 0) {
+                T.deallocate();
+                return false;
+            }
+
+            for (j = 0; j < size; ++j) {
+                if (i != j) {
+                    double S = T.value(j, i) / T.value(i, i);
+                    for (k = 0; k < size * 2; ++k) {
+                        *T.ptr(j, k) = T.value(j, k) - S * T.value(i, k);
+                    }
+                }
+            }
+        }
+
+        //log_debug( "After elimination" );
+        //T.dump();
+        for (i = 0; i < size; ++i) {
+            for (j = 0; j < size; ++j)
+                *B->ptr(i, j) = static_cast<float>(T.value(i, j + size) / T.value(i, i));
+        }
+
+        T.deallocate();
+        return true;
+    }
+
+    template void GenericCartesian::Mtx<float>::transform(float* to, const float* from) const;
+    template void GenericCartesian::Mtx<float>::dump() const;
+
+}
diff --git a/FluidNC/src/Kinematics/GenericCartesian.h b/FluidNC/src/Kinematics/GenericCartesian.h
new file mode 100644
index 0000000000..3e0e67fcf5
--- /dev/null
+++ b/FluidNC/src/Kinematics/GenericCartesian.h
@@ -0,0 +1,81 @@
+// Copyright (c) 2020 -	Bart Dring
+// Copyright (c) 2023 -	Vlad A.
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#pragma once
+
+/*
+	GenericCartesian.h
+
+	This is a kinematic system for where the motors operate in the cartesian space.
+
+    An abstract class which should be a base for diferent transformation cases.
+
+    Unlike Cartisian kinematic system which uses the identity transform between the
+    axis and motor spaces, this system uses an arbitrary linear transform 
+    defined by an invertible matrix.
+
+*/
+
+#include "Kinematics.h"
+
+namespace Kinematics {
+
+    class GenericCartesian : public KinematicSystem {
+    public:
+        GenericCartesian() = default;
+
+        GenericCartesian(const GenericCartesian&)            = delete;
+        GenericCartesian(GenericCartesian&&)                 = delete;
+        GenericCartesian& operator=(const GenericCartesian&) = delete;
+        GenericCartesian& operator=(GenericCartesian&&)      = delete;
+
+        // Kinematic Interface
+        virtual void init() override;
+        virtual void init_position() override;
+
+        virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override;
+        void         motors_to_cartesian(float* cartesian, float* motors, int n_axis) override;
+        void         transform_cartesian_to_motors(float* cartesian, float* motors) override;
+
+        bool canHome(AxisMask axisMask) override;
+        void releaseMotors(AxisMask axisMask, MotorMask motors) override;
+        bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override;
+
+    protected:
+        template <typename number>
+        class Mtx {
+            size_t  _pitch; // Here, it's equal the number of columns in a matrix row
+            size_t  _lines; // The number of rows
+            number* _buffer;
+
+        public:
+            Mtx(const size_t row, const size_t col) : _pitch(col), _lines(row) { _buffer = new number[_pitch * _lines]; };
+
+            void allocate() {}
+            void deallocate() {}
+
+            number* getBuffer() { return _buffer; }
+            number  value(const size_t row, const size_t col) const { return _buffer[row * _pitch + col]; }
+            number* ptr(const size_t row, const size_t col) { return _buffer + row * _pitch + col; }
+            void    transform(number* to, const number* from) const;
+
+            void dumpRow(const size_t idx) const;
+            void dump() const;
+
+            ~Mtx() {
+                if (_buffer) {
+                    delete[] _buffer;
+                }
+            }
+        };
+
+        float       _buffer[6];
+        Mtx<float>* _mtx = nullptr;
+        Mtx<float>* _rev = nullptr;
+
+        bool GJ_invertMatrix(const size_t size, const Mtx<float>* const A, Mtx<float>* const B);
+
+        ~GenericCartesian();
+    };
+}  //  namespace Kinematics
diff --git a/FluidNC/src/Kinematics/Skewed.cpp b/FluidNC/src/Kinematics/Skewed.cpp
new file mode 100644
index 0000000000..f55a51d007
--- /dev/null
+++ b/FluidNC/src/Kinematics/Skewed.cpp
@@ -0,0 +1,129 @@
+#include "Skewed.h"
+
+#include "src/Machine/MachineConfig.h"
+#include "src/Machine/Axes.h"  // ambiguousLimit()
+#include "src/Limits.h"
+
+namespace Kinematics {
+
+    void SkewAxis::group(Configuration::HandlerBase& handler) {
+        handler.item("distance_mm", _dist, 1.0f, 100000.0f );
+        handler.item("offset_x_mm", _offsets[0], -1000.0f, 1000.0f );
+        handler.item("offset_y_mm", _offsets[1], -1000.0f, 1000.0f );
+        handler.item("offset_z_mm", _offsets[2], -1000.0f, 1000.0f );
+        handler.item("offset_a_mm", _offsets[3], -1000.0f, 1000.0f );
+        handler.item("offset_b_mm", _offsets[4], -1000.0f, 1000.0f );
+        handler.item("offset_c_mm", _offsets[5], -1000.0f, 1000.0f );
+    }
+
+    void SkewAxis::init() {
+        log_debug( "      Skew ( " << _offsets[0] << ", " << _offsets[1] << ", " << _offsets[2] << " ) over " << _dist << "mm" );
+    }
+
+
+    ////////////////////
+
+    Skewed::Skewed() : _numberSkewAxis( MAX_N_AXIS ) {
+        for (int i = 0; i < MAX_N_AXIS; ++i) {
+            _skewAxis[i] = nullptr;
+        }
+
+    }
+
+    void Skewed::init() {
+        GenericCartesian::init();
+
+        bool fail = false;
+
+        if (_mtx)
+            delete _mtx;
+
+        if (_rev)
+            delete _rev;
+
+        _mtx = new Mtx<float>( _numberSkewAxis, _numberSkewAxis );
+        _rev = new Mtx<float>( _numberSkewAxis, _numberSkewAxis );
+
+        for (size_t axis = 0; axis < _numberSkewAxis; axis++) {
+            auto a = _skewAxis[axis];
+            if (a) {
+                a->init();
+                
+                float* row = _mtx->ptr( axis, 0 );
+                const SkewAxis* skew = _skewAxis[axis];
+
+                for( uint i = 0; i < _numberSkewAxis; ++i ) {
+                    row[ i ]  = skew->_offsets[ i ] / skew->_dist;
+                    row[ i ] += ( i == axis ) ? 1.0f : 0.0f;
+                }
+            } else {
+                fail = true;
+                break;
+            }
+        }
+
+        if (!fail)
+            fail = ! GJ_invertMatrix( _numberSkewAxis, _mtx, _rev );
+
+        if (!fail) {
+            log_debug("Direct transform");
+            _mtx->dump();
+            log_debug("Reverse transform");
+            _rev->dump();
+        } else {
+            log_warn("Fail during building transformation matrices. Probably skew settings are too wild. Skew correction will be "
+                     "disabled.");
+            
+            if (_mtx) {
+                delete _mtx;
+                _mtx = nullptr;
+            }
+
+            if (_rev) {
+                delete _rev;
+                _rev = nullptr;
+            }
+        }
+    }
+
+    void Skewed::afterParse() {
+        // Find the last axis that was declared and set _numberSkewAxis accordingly
+        for (size_t i = MAX_N_AXIS; i > 0; --i) {
+            if (_skewAxis[i - 1] != nullptr) {
+                _numberSkewAxis = i;
+                break;
+            }
+        }
+        // Senders might assume 3 axes in reports
+        if (_numberSkewAxis < 3) {
+            _numberSkewAxis = 3;
+        }
+
+        for (size_t i = 0; i < _numberSkewAxis; ++i) {
+            if (_skewAxis[i] == nullptr) {
+                _skewAxis[i] = new SkewAxis();
+            }
+        }
+    }
+
+    void Skewed::group(Configuration::HandlerBase& handler) {
+        char   tmp[2];
+        size_t n_axis = _numberSkewAxis ? _numberSkewAxis : MAX_N_AXIS;
+
+        for (size_t i = 0; i < n_axis; ++i) {
+            tmp[0] = config->_axes->axisName( i );
+            tmp[1] = 0;
+
+            handler.section(tmp, _skewAxis[i]);
+        }
+    }
+
+    void Skewed::validate() {
+        init();
+    }
+
+    // Configuration registration
+    namespace {
+        KinematicsFactory::InstanceBuilder<Skewed> registration("Skewed");
+    }
+}
diff --git a/FluidNC/src/Kinematics/Skewed.h b/FluidNC/src/Kinematics/Skewed.h
new file mode 100644
index 0000000000..96113248ed
--- /dev/null
+++ b/FluidNC/src/Kinematics/Skewed.h
@@ -0,0 +1,61 @@
+// Copyright (c) 2020 -	Bart Dring
+// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file.
+
+#pragma once
+
+/*
+	Skewed.h
+
+	This is a kinematic system for where the motors operate in the cartesian space. 
+    But unlike Cartesian kinematic system, it adds ability to tweak skew correction, 
+    for cases when physical geometry of CNC machine is not ideal.
+*/
+
+#include "GenericCartesian.h"
+
+namespace Kinematics {
+    class SkewAxis  : public Configuration::Configurable {
+    public:
+        SkewAxis() { _offsets[5] = _offsets[4] = _offsets[3] = _offsets[2] = _offsets[1] = _offsets[0] = 0.0f; };
+
+        float    _dist     = 10.0f;
+        float    _offsets[ 6 ];
+
+        // Configuration system helpers:
+        void validate() override {};
+        void afterParse() override {};
+        void group(Configuration::HandlerBase& handler) override;
+
+        void init();
+    };
+
+
+    class Skewed : public GenericCartesian {
+        uint _numberSkewAxis;
+
+    public:
+        Skewed();
+
+        Skewed(const Skewed&)            = delete;
+        Skewed(Skewed&&)                 = delete;
+        Skewed& operator=(const Skewed&) = delete;
+        Skewed& operator=(Skewed&&)      = delete;
+
+        // Kinematic Interface
+        virtual void init() override;
+
+        // Configuration handlers:
+        void afterParse() override;
+        void group(Configuration::HandlerBase& handler) override;
+        void validate() override;
+
+        // Name of the configurable. Must match the name registered in the cpp file.
+        const char* name() const override { return "Skew corrected Cartesian"; }
+
+    protected:
+        SkewAxis* _skewAxis[6];
+//        float _buffer[6];
+
+        ~Skewed() {}
+    };
+}  //  namespace Kinematics
diff --git a/FluidNC/src/Machine/MachineConfig.h b/FluidNC/src/Machine/MachineConfig.h
index 16c0673e7d..213e0cd980 100644
--- a/FluidNC/src/Machine/MachineConfig.h
+++ b/FluidNC/src/Machine/MachineConfig.h
@@ -117,7 +117,7 @@ namespace Machine {
 extern Machine::MachineConfig* config;
 
 template <typename T>
-void copyAxes(T* dest, T* src) {
+void copyAxes(T* dest, const T* src) {
     auto n_axis = config->_axes->_numberAxis;
     for (size_t axis = 0; axis < n_axis; axis++) {
         dest[axis] = src[axis];
diff --git a/FluidNC/test/Configuration/YamlTreeBuilder.cpp b/FluidNC/test/Configuration/YamlTreeBuilder.cpp
index 189d70b855..de9a70d4fb 100644
--- a/FluidNC/test/Configuration/YamlTreeBuilder.cpp
+++ b/FluidNC/test/Configuration/YamlTreeBuilder.cpp
@@ -14,7 +14,7 @@ namespace Configuration {
         String b;
         String c;
 
-        void validate() const {}
+        void validate() {}
         void group(HandlerBase& handler) {
             handler.item("a", a);
             handler.item("b", b);
@@ -27,7 +27,7 @@ namespace Configuration {
         String aap;
         int    banaan;
 
-        void validate() const {}
+        void validate() {}
         void group(HandlerBase& handler) {
             handler.item("aap", aap);
             handler.item("banaan", banaan);
@@ -51,7 +51,7 @@ namespace Configuration {
         int value;
         int banaan;
 
-        void validate() const {}
+        void validate() {}
         void group(HandlerBase& handler) override {
             handler.item("aap", aap);
             handler.item("type", value, stepTypes);
@@ -65,7 +65,7 @@ namespace Configuration {
         TestBasic2* n2  = nullptr;
         int         foo = 0;
 
-        void validate() const {}
+        void validate() {}
         void group(HandlerBase& handler) override {
             handler.section("n1", n1);
             handler.section("n2", n2);