diff --git a/.gitignore b/.gitignore
index 04fe264b6b..09b6888cc8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
 *.DS_Store
+*.gcov
 *~
 .cache/
 .pytest_cache/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 858e44b144..9e4c0a14d0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,6 +35,13 @@ cmake_minimum_required(VERSION 3.0)
 
 project(ECL CXX)
 
+if (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
+    message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
+endif()
+
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage")
+
 execute_process(
 	COMMAND git describe --always --tags
 	OUTPUT_VARIABLE git_tag
@@ -48,6 +55,18 @@ set(CMAKE_CXX_STANDARD 11)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
+# code coverage support
+option(COV_HTML "Display html for coverage" OFF)
+
+set(CMAKE_CXX_FLAGS_COVERAGE
+    "--coverage -fprofile-arcs -ftest-coverage -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors"
+    CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
+set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+    "--coverage -ftest-coverage -lgcov"
+    CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
+mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE)
+
+
 set(ECL_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR})
 
 if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
@@ -79,21 +98,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 		)
 	endif()
 
-	# fetch latest matrix from github
-	include(ExternalProject)
-	ExternalProject_Add(matrix
-			GIT_REPOSITORY "https://github.com/PX4/Matrix.git"
-			UPDATE_COMMAND ""
-			PATCH_COMMAND ""
-			CONFIGURE_COMMAND ""
-			BUILD_COMMAND ""
-			INSTALL_COMMAND ""
-			)
-	add_dependencies(prebuild_targets matrix)
-	include_directories(${CMAKE_BINARY_DIR}/matrix-prefix/src/matrix)
-
-	add_subdirectory(mathlib)
-
+	# testing
 	include(CTest)
 	enable_testing()
 	
@@ -113,6 +118,22 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 		endif()
 	endif()
 
+	# fetch latest matrix from github
+	include(ExternalProject)
+	ExternalProject_Add(matrix
+			GIT_REPOSITORY "https://github.com/PX4/Matrix.git"
+			UPDATE_COMMAND ""
+			PATCH_COMMAND ""
+			CONFIGURE_COMMAND ""
+			BUILD_COMMAND ""
+			INSTALL_COMMAND ""
+			)
+	add_dependencies(prebuild_targets matrix)
+	include_directories(${CMAKE_BINARY_DIR}/matrix-prefix/src/matrix)
+
+	# mathlib only needed in standalone build
+	add_subdirectory(mathlib)
+
 endif()
 
 add_subdirectory(airdata)
@@ -124,6 +145,29 @@ add_subdirectory(l1)
 add_subdirectory(tecs)
 add_subdirectory(validation)
 
+#=============================================================================
+# Coverage
+#
+if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage")
+
+    add_custom_target(coverage
+        COMMAND ${CMAKE_CTEST_COMMAND}
+        COMMAND lcov --capture --directory . --output-file coverage.info
+        COMMAND lcov --remove coverage.info '/usr/*' --output-file coverage.info # filter out system
+        COMMAND lcov --remove coverage.info 'build/coverage_build/EKF/swig/*' --output-file coverage.info
+        COMMAND lcov --summary coverage.info
+        WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+        DEPENDS check
+        )
+
+    add_custom_target(coverage_html
+        COMMAND genhtml coverage.info --output-directory out
+        WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+        DEPENDS coverage
+        )
+
+endif()
+
 #=============================================================================
 # Doxygen
 #
diff --git a/EKF/tests/CMakeLists.txt b/EKF/tests/CMakeLists.txt
index 7f7dc8e019..2205f8a8cf 100644
--- a/EKF/tests/CMakeLists.txt
+++ b/EKF/tests/CMakeLists.txt
@@ -32,6 +32,9 @@
 ############################################################################
 
 if(BUILD_TESTING AND ECL_STANDALONE)
+
+	add_definitions(-UNDEBUG) # keep assert
+
 	add_subdirectory(base)
 	
 	if(EKF_PYTHON_TESTS)
diff --git a/Jenkinsfile b/Jenkinsfile
index 6fbd0346a8..0d9620fd8b 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -71,6 +71,35 @@ pipeline {
     stage('Test') {
       parallel {
 
+        stage('coverage') {
+          agent {
+            docker {
+              image 'px4io/px4-dev-ecl:2018-04-22'
+              args '-v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+            }
+          }
+          steps {
+            sh 'export'
+            sh 'ccache -z'
+            sh 'make distclean'
+            sh 'make coverage'
+            //sh 'bash <(curl -s https://codecov.io/bash) -t ${CODECOV_TOKEN}'
+            sh 'make coverage_html'
+            // publish html
+            publishHTML target: [
+              reportTitles: 'code coverage',
+              allowMissing: false,
+              alwaysLinkToLastBuild: true,
+              keepAll: true,
+              reportDir: 'build/coverage_build/out',
+              reportFiles: '*',
+              reportName: 'Code Coverage'
+            ]
+            sh 'ccache -s'
+            sh 'make distclean'
+          }
+        }
+
         stage('EKF pytest') {
           agent {
             docker {
@@ -84,7 +113,7 @@ pipeline {
             sh 'make distclean'
             sh 'make test_EKF'
             sh 'ccache -s'
-            archiveArtifacts(artifacts: 'build/**/*.pdf')
+            archiveArtifacts(artifacts: 'build/test_build/*.pdf')
             sh 'make distclean'
           }
         }
diff --git a/Makefile b/Makefile
index 947fe823d2..98cea21dea 100644
--- a/Makefile
+++ b/Makefile
@@ -78,7 +78,7 @@ SRC_DIR := $(shell dirname $(realpath $(lastword $(MAKEFILE_LIST))))
 define cmake-build
 +@$(eval BUILD_DIR = $(SRC_DIR)/build/$@$(BUILD_DIR_SUFFIX))
 +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
-+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(2) -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) $(3) || (rm -rf $(BUILD_DIR)); fi
++@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(2) -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) $(3) $(4) || (rm -rf $(BUILD_DIR)); fi
 +@(cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
@@ -109,6 +109,19 @@ test_EKF: test_build
 test_EKF_plots: test_build
 	@cmake --build $(SRC_DIR)/build/test_build --target ecl_EKF_pytest-plots
 
+
+# Code coverage
+# --------------------------------------------------------------------
+
+coverage_build:
+	@$(call cmake-build,$@,$(SRC_DIR), "-DCMAKE_BUILD_TYPE=Coverage", "-DEKF_PYTHON_TESTS=ON")
+	
+coverage: coverage_build
+	@cmake --build $(SRC_DIR)/build/coverage_build --target coverage
+
+coverage_html: coverage
+	@cmake --build $(SRC_DIR)/build/coverage_build --target coverage_html
+
 # Cleanup
 # --------------------------------------------------------------------
 .PHONY: clean distclean