diff --git a/examples/positioning/bezier_trajectory.py b/examples/positioning/bezier_trajectory.py
new file mode 100644
index 0000000000000000000000000000000000000000..06f2a36f58e5b3923f75513864677e8bd3b56e7e
--- /dev/null
+++ b/examples/positioning/bezier_trajectory.py
@@ -0,0 +1,390 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2019 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+Example of how to generate trajectories for the High Level commander using
+Bezier curves. The output from this script is intended to be pasted into the
+autonomous_sequence_high_level.py example.
+
+This code uses Bezier curves of degree 7, that is with 8 control points.
+See https://en.wikipedia.org/wiki/B%C3%A9zier_curve
+
+All coordinates are (x, y, z, yaw)
+
+Limitations: all Segments must use the same scale
+"""
+import math
+
+import numpy as np
+
+
+class Node:
+    """
+    A node represents the connection point between two Bezier curves
+    (called Segments).
+    It holds 4 control points for each curve and the positions of the control
+    points are set to join the curves with continuity in c0, c1, c2, c3.
+    See https://www.cl.cam.ac.uk/teaching/2000/AGraphHCI/SMEG/node3.html
+
+    The control points are named
+    p4, p5, p6 and p7 for the tail of the first curve
+    q0, q1, q2, q3 for the head of the second curve
+    """
+
+    def __init__(self, q0, q1=None, q2=None, q3=None):
+        """
+        Create a Node. Pass in control points to define the shape of the
+        two segments that share the Node. The control points are for the
+        second segment, that is the four first control points of the Bezier
+        curve after the node. The control points for the Bezier curve before
+        the node are calculated from the existing control points.
+
+        q0 is required, the other points are optional.
+        if q1 is missing it will be set to generate no velocity in q0.
+        If q2 is missing it will be set to generate no acceleration in q0.
+        If q3 is missing it will be set to generate no jerk in q0.
+
+        If only q0 is set, the node will represent a point where the Crazyflie
+        has no velocity. Good for starting and stopping.
+
+        To get a fluid motion between segments, q1 must be set.
+
+        :param q0: The position of the node
+        :param q1: The position of the first control point
+        :param q2: The position of the second control point
+        :param q3: The position of the third control point
+        """
+        self._control_points = np.zeros([2, 4, 4])
+
+        q0 = np.array(q0)
+
+        if q1 is None:
+            q1 = q0
+        else:
+            q1 = np.array(q1)
+            # print('q1 generated:', q1)
+
+        d = q1 - q0
+
+        if q2 is None:
+            q2 = q0 + 2 * d
+            # print('q2 generated:', q2)
+        else:
+            q2 = np.array(q2)
+
+        e = 3 * q2 - 2 * q0 - 6 * d
+
+        if q3 is None:
+            q3 = e + 3 * d
+            # print('q3 generated:', q3)
+        else:
+            q3 = np.array(q3)
+
+        p7 = q0
+        p6 = q1 - 2 * d
+        p5 = q2 - 4 * d
+        p4 = 2 * e - q3
+
+        self._control_points[0][0] = q0
+        self._control_points[0][1] = q1
+        self._control_points[0][2] = q2
+        self._control_points[0][3] = q3
+
+        self._control_points[1][3] = p7
+        self._control_points[1][2] = p6
+        self._control_points[1][1] = p5
+        self._control_points[1][0] = p4
+
+    def get_head_points(self):
+        return self._control_points[0]
+
+    def get_tail_points(self):
+        return self._control_points[1]
+
+    def print(self):
+        print('Node ---')
+        print('Tail:')
+        for c in self._control_points[1]:
+            print(c)
+        print('Head:')
+        for c in self._control_points[0]:
+            print(c)
+
+
+class Segment:
+    """
+    A Segment represents a Bezier curve of degree 7. It uses two Nodes to
+    define the shape.
+    A Segment can generate a polynomial that is compatible with the High Level
+    Commander, either in python to be sent to the Crazyflie, or as C code to be
+    used in firmware.
+    A Segment can also be rendered in Vispy.
+
+    Note: all segments must use the same scale
+    """
+
+    def __init__(self, head_node, tail_node, scale):
+        self._scale = scale
+
+        self._points = np.concatenate(
+            [head_node.get_head_points(), tail_node.get_tail_points()])
+
+        polys = self._convert_to_polys()
+        self._polys = self._stretch_polys(polys, self._scale)
+
+        self._vel = self._deriv(self._polys)
+        self._acc = self._deriv(self._vel)
+        self._jerk = self._deriv(self._acc)
+
+    def _convert_to_polys(self):
+        n = len(self._points) - 1
+        result = np.zeros([4, 8])
+
+        for d in range(4):
+            for j in range(n + 1):
+                s = 0.0
+                for i in range(j + 1):
+                    s += ((-1) ** (i + j)) * self._points[i][d] / (
+                        math.factorial(i) * math.factorial(j - i))
+
+                c = s * math.factorial(n) / math.factorial(n - j)
+                result[d][j] = c
+
+        return result
+
+    def draw_trajectory(self, visualizer):
+        self._draw(self._polys, 'black', visualizer)
+
+    def draw_vel(self, visualizer):
+        self._draw(self._vel, 'green', visualizer)
+
+    def draw_acc(self, visualizer):
+        self._draw(self._acc, 'red', visualizer)
+
+    def draw_jerk(self, visualizer):
+        self._draw(self._jerk, 'blue', visualizer)
+
+    def draw_control_points(self, visualizer):
+        for p in self._points:
+            visualizer.marker(p[0:3])
+
+    def _draw(self, polys, color, visualizer):
+        step = self._scale / 32
+        prev = None
+        for t in np.arange(0.0, self._scale + step, step):
+            p = self._eval_xyz(polys, t)
+
+            if prev is not None:
+                visualizer.line(p, prev, color=color)
+
+            prev = p
+
+    def velocity(self, t):
+        return self._eval_xyz(self._vel, t)
+
+    def acceleration(self, t):
+        return self._eval_xyz(self._acc, t)
+
+    def jerk(self, t):
+        return self._eval_xyz(self._jerk, t)
+
+    def _deriv(self, p):
+        result = []
+        for i in range(3):
+            result.append([
+                1 * p[i][1],
+                2 * p[i][2],
+                3 * p[i][3],
+                4 * p[i][4],
+                5 * p[i][5],
+                6 * p[i][6],
+                7 * p[i][7],
+                0
+            ])
+
+        return result
+
+    def _eval(self, p, t):
+        result = 0
+        for part in range(8):
+            result += p[part] * (t ** part)
+        return result
+
+    def _eval_xyz(self, p, t):
+        return np.array(
+            [self._eval(p[0], t), self._eval(p[1], t), self._eval(p[2], t)])
+
+    def print_poly_python(self):
+        s = '  [' + str(self._scale) + ', '
+
+        for axis in range(4):
+            for d in range(8):
+                s += str(self._polys[axis][d]) + ', '
+
+        s += '],  # noqa'
+        print(s)
+
+    def print_poly_c(self):
+        s = ''
+
+        for axis in range(4):
+            for d in range(8):
+                s += str(self._polys[axis][d]) + ', '
+
+        s += str(self._scale)
+        print(s)
+
+    def print_points(self):
+        print(self._points)
+
+    def print_peak_vals(self):
+        peak_v = 0.0
+        peak_a = 0.0
+        peak_j = 0.0
+
+        step = 0.05
+        for t in np.arange(0.0, self._scale + step, step):
+            peak_v = max(peak_v, np.linalg.norm(self._eval_xyz(self._vel, t)))
+            peak_a = max(peak_a, np.linalg.norm(self._eval_xyz(self._acc, t)))
+            peak_j = max(peak_j, np.linalg.norm(self._eval_xyz(self._jerk, t)))
+
+        print('Peak v:', peak_v, 'a:', peak_a, 'j:', peak_j)
+
+    def _stretch_polys(self, polys, time):
+        result = np.zeros([4, 8])
+
+        recip = 1.0 / time
+
+        for p in range(4):
+            scale = 1.0
+            for t in range(8):
+                result[p][t] = polys[p][t] * scale
+                scale *= recip
+
+        return result
+
+
+class Visualizer:
+    def __init__(self):
+        self.canvas = scene.SceneCanvas(keys='interactive', size=(800, 600),
+                                        show=True)
+        view = self.canvas.central_widget.add_view()
+        view.bgcolor = '#ffffff'
+        view.camera = TurntableCamera(fov=10.0, distance=40.0, up='+z',
+                                      center=(0.0, 0.0, 1.0))
+        XYZAxis(parent=view.scene)
+        self.scene = view.scene
+
+    def marker(self, pos, color='black', size=8):
+        Markers(pos=np.array(pos, ndmin=2), face_color=color,
+                parent=self.scene, size=size)
+
+    def lines(self, points, color='black'):
+        LinePlot(points, color=color, parent=self.scene)
+
+    def line(self, a, b, color='black'):
+        self.lines([a, b], color)
+
+    def run(self):
+        self.canvas.app.run()
+
+
+segment_time = 2
+z = 1
+yaw = 0
+
+segments = []
+
+# Nodes with one control point has not velocity, this is similar to calling
+# goto in the High-level commander
+
+n0 = Node((0, 0, z, yaw))
+n1 = Node((1, 0, z, yaw))
+n2 = Node((1, 1, z, yaw))
+n3 = Node((0, 1, z, yaw))
+
+segments.append(Segment(n0, n1, segment_time))
+segments.append(Segment(n1, n2, segment_time))
+segments.append(Segment(n2, n3, segment_time))
+segments.append(Segment(n3, n0, segment_time))
+
+
+# By setting the q1 control point we get velocity through the nodes
+# Increase d to 0.7 to get some more action
+d = 0.1
+
+n5 = Node((1, 0, z, yaw), q1=(1 + d, 0 + d, z, yaw))
+n6 = Node((1, 1, z, yaw), q1=(1 - d, 1 + d, z, yaw))
+n7 = Node((0, 1, z, yaw), q1=(0 - d, 1 - d, z, yaw))
+
+segments.append(Segment(n0, n5, segment_time))
+segments.append(Segment(n5, n6, segment_time))
+segments.append(Segment(n6, n7, segment_time))
+segments.append(Segment(n7, n0, segment_time))
+
+
+# When setting q2 we can also control acceleration and get more action.
+# Yaw also adds to the fun.
+
+d2 = 0.4
+dyaw = 3
+f = -1.0
+
+n8 = Node(
+    (1, 0, z, yaw),
+    q1=(1 + d2, 0 + d2, z, yaw),
+    q2=(1 + 2 * d2, 0 + 2 * d2 + 0*f * d2, 1, yaw))
+n9 = Node(
+    (1, 1, z, yaw + dyaw),
+    q1=(1 - d2, 1 + d2, z, yaw + dyaw),
+    q2=(1 - 2 * d2 + f * d2, 1 + 2 * d2 + f * d2, 1, yaw + dyaw))
+n10 = Node(
+    (0, 1, z, yaw - dyaw),
+    q1=(0 - d2, 1 - d2, z, yaw - dyaw),
+    q2=(0 - 2 * d2,  1 - 2 * d2,  1, yaw - dyaw))
+
+segments.append(Segment(n0, n8, segment_time))
+segments.append(Segment(n8, n9, segment_time))
+segments.append(Segment(n9, n10, segment_time))
+segments.append(Segment(n10, n0, segment_time))
+
+
+print('Paste this code into the autonomous_sequence_high_level.py example to '
+      'see it fly')
+for s in segments:
+    s.print_poly_python()
+
+
+# Enable this if you have Vispy installed and want a visualization of the
+# trajectory
+if False:
+    # Import here to avoid problems for users that does not have Vispy
+    from vispy import scene
+    from vispy.scene import XYZAxis, LinePlot, TurntableCamera, Markers
+
+    visualizer = Visualizer()
+    for s in segments:
+        s.draw_trajectory(visualizer)
+        # s.draw_vel(visualizer)
+        # s.draw_control_points(visualizer)
+    visualizer.run()