Add an adaptive servo based on linear regression.

This servo uses linear regression to estimate current time and
frequency error. The number of points used in the regression is
variable (from 4 to 64 in powers of 2) and is selected by a long-term
statistic of the prediction error.

Future improvements could include tracking of sudden frequency changes
(e.g. due to temperature variations), better stability of the error
statistic when a large offset is corrected, options to set the speed of
the adaptation, minimum and maximum number of points, or an option to
prefer frequency accuracy over time accuracy.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>

config.c

@@ -497,6 +497,8 @@ static enum parser_result parse_global_setting(const char *option,
 	} else if (!strcmp(option, "clock_servo")) {
 		if (!strcasecmp("pi", value))
 			cfg->clock_servo = CLOCK_SERVO_PI;
+		else if (!strcasecmp("linreg", value))
+			cfg->clock_servo = CLOCK_SERVO_LINREG;
 		else
 			return BAD_VALUE;
 

linreg.c

@@ -0,0 +1,312 @@
+/**
+ * @file linreg.c
+ * @brief Implements an adaptive servo based on linear regression.
+ * @note Copyright (C) 2014 Miroslav Lichvar <mlichvar@redhat.com>
+ *
+ * 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.
+ */
+#include <stdlib.h>
+#include <math.h>
+
+#include "linreg.h"
+#include "print.h"
+#include "servo_private.h"
+
+/* Maximum and minimum number of points used in regression,
+   defined as a power of 2 */
+#define MAX_SIZE 6
+#define MIN_SIZE 2
+
+#define MAX_POINTS (1 << MAX_SIZE)
+
+/* Smoothing factor used for long-term prediction error */
+#define ERR_SMOOTH 0.02
+/* Number of updates used for initialization */
+#define ERR_INITIAL_UPDATES 10
+/* Maximum ratio of two err values to be considered equal */
+#define ERR_EQUALS 1.05
+
+/* Uncorrected local time vs remote time */
+struct point {
+	uint64_t x;
+	uint64_t y;
+};
+
+struct result {
+	/* Slope and intercept from latest regression */
+	double slope;
+	double intercept;
+	/* Exponential moving average of prediction error */
+	double err;
+	/* Number of initial err updates */
+	int err_updates;
+};
+
+struct linreg_servo {
+	struct servo servo;
+	/* Circular buffer of points */
+	struct point points[MAX_POINTS];
+	/* Current time in x, y */
+	struct point reference;
+	/* Number of stored points */
+	unsigned int num_points;
+	/* Index of the newest point */
+	unsigned int last_point;
+	/* Remainder from last update of reference.x */
+	double x_remainder;
+	/* Local time stamp of last update */
+	uint64_t last_update;
+	/* Regression results for all sizes */
+	struct result results[MAX_SIZE - MIN_SIZE + 1];
+	/* Current frequency offset of the clock */
+	double clock_freq;
+	/* Expected interval between updates */
+	double update_interval;
+};
+
+static void linreg_destroy(struct servo *servo)
+{
+	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+	free(s);
+}
+
+static void move_reference(struct linreg_servo *s, int64_t x, int64_t y)
+{
+	struct result *res;
+	unsigned int i;
+
+	s->reference.x += x;
+	s->reference.y += y;
+
+	/* Update intercepts for new reference */
+	for (i = MIN_SIZE; i <= MAX_SIZE; i++) {
+		res = &s->results[i - MIN_SIZE];
+		res->intercept += x * res->slope - y;
+	}
+}
+
+static void update_reference(struct linreg_servo *s, uint64_t local_ts)
+{
+	double x_interval;
+	int64_t y_interval;
+
+	if (s->last_update) {
+		y_interval = local_ts - s->last_update;
+
+		/* Remove current frequency correction from the interval */
+		x_interval = y_interval / (1.0 + s->clock_freq / 1e9);
+		x_interval += s->x_remainder;
+		s->x_remainder = x_interval - (int64_t)x_interval;
+
+		move_reference(s, (int64_t)x_interval, y_interval);
+	}
+
+	s->last_update = local_ts;
+}
+
+static void add_sample(struct linreg_servo *s, int64_t offset)
+{
+	s->last_point = (s->last_point + 1) % MAX_POINTS;
+
+	s->points[s->last_point].x = s->reference.x;
+	s->points[s->last_point].y = s->reference.y - offset;
+
+	if (s->num_points < MAX_POINTS)
+		s->num_points++;
+}
+
+static void regress(struct linreg_servo *s)
+{
+	double x, y, y0, e, x_sum, y_sum, xy_sum, x2_sum;
+	unsigned int i, l, n, size;
+	struct result *res;
+
+	x_sum = 0.0, y_sum = 0.0, xy_sum = 0.0, x2_sum = 0.0;
+	i = 0;
+
+	y0 = (int64_t)(s->points[s->last_point].y - s->reference.y);
+
+	for (size = MIN_SIZE; size <= MAX_SIZE; size++) {
+		n = 1 << size;
+		if (n > s->num_points)
+			/* Not enough points for this size */
+			break;
+
+		res = &s->results[size - MIN_SIZE];
+
+		/* Update moving average of the prediction error */
+		if (res->slope) {
+			e = fabs(res->intercept - y0);
+			if (res->err_updates < ERR_INITIAL_UPDATES) {
+				res->err *= res->err_updates;
+				res->err += e;
+				res->err_updates++;
+				res->err /= res->err_updates;
+			} else {
+				res->err += ERR_SMOOTH * (e - res->err);
+			}
+		}
+
+		for (; i < n; i++) {
+			/* Iterate points from newest to oldest */
+			l = (MAX_POINTS + s->last_point - i) % MAX_POINTS;
+
+			x = (int64_t)(s->points[l].x - s->reference.x);
+			y = (int64_t)(s->points[l].y - s->reference.y);
+
+			x_sum += x;
+			y_sum += y;
+			xy_sum += x * y;
+			x2_sum += x * x;
+		}
+
+		/* Get new intercept and slope */
+		res->slope = (xy_sum - x_sum * y_sum / n) /
+				(x2_sum - x_sum * x_sum / n);
+		res->intercept = (y_sum - res->slope * x_sum) / n;
+	}
+}
+
+/* Return largest size with smallest prediction error */
+static int get_best_size(struct linreg_servo *s)
+{
+	struct result *res;
+	double best_err;
+	int size, best_size;
+
+	best_size = 0;
+	best_err = 0.0;
+
+	for (size = MIN_SIZE; size <= MAX_SIZE; size++) {
+		res = &s->results[size - MIN_SIZE];
+		if ((!best_size && res->slope) ||
+		    (best_err * ERR_EQUALS > res->err &&
+		     res->err_updates >= ERR_INITIAL_UPDATES)) {
+			best_size = size;
+			best_err = res->err;
+		}
+	}
+
+	return best_size;
+}
+
+static double linreg_sample(struct servo *servo,
+			    int64_t offset,
+			    uint64_t local_ts,
+			    enum servo_state *state)
+{
+	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+	struct result *res;
+	int size, corr_interval;
+
+	/*
+	 * The current time and the time when will be the frequency of the
+	 * clock actually updated is assumed here to be equal to local_ts
+	 * (which is the time stamp of the received sync message). As long as
+	 * the differences are smaller than the update interval, the loop
+	 * should be robust enough to handle this simplification.
+	 */
+
+	update_reference(s, local_ts);
+	add_sample(s, offset);
+	regress(s);
+
+	size = get_best_size(s);
+
+	if (size < MIN_SIZE) {
+		/* Not enough points, wait for more */
+		*state = SERVO_UNLOCKED;
+		return -s->clock_freq;
+	}
+
+	res = &s->results[size - MIN_SIZE];
+
+	pr_debug("linreg: points %d slope %.9f intercept %.0f err %.0f",
+			1 << size, res->slope, res->intercept, res->err);
+
+	if ((servo->first_update &&
+	     servo->first_step_threshold &&
+	     servo->first_step_threshold < fabs(res->intercept)) ||
+	    (servo->step_threshold &&
+	     servo->step_threshold < fabs(res->intercept))) {
+		/* The clock will be stepped by offset */
+		move_reference(s, 0, -offset);
+		s->last_update -= offset;
+		*state = SERVO_JUMP;
+	} else {
+		*state = SERVO_LOCKED;
+	}
+
+	/* Set clock frequency to the slope */
+	s->clock_freq = 1e9 * (res->slope - 1.0);
+
+	/*
+	 * Adjust the frequency to correct the time offset. Use longer
+	 * correction interval with larger sizes to reduce the frequency error.
+	 * The update interval is assumed to be not affected by the frequency
+	 * adjustment. If it is (e.g. phc2sys controlling the system clock), a
+	 * correction slowing down the clock will result in an overshoot. With
+	 * the system clock's maximum adjustment of 10% that's acceptable.
+	 */
+	corr_interval = size <= 4 ? 1 : size / 2;
+	s->clock_freq += res->intercept / s->update_interval / corr_interval;
+
+	/* Clamp the frequency to the allowed maximum */
+	if (s->clock_freq > servo->max_frequency)
+		s->clock_freq = servo->max_frequency;
+	else if (s->clock_freq < -servo->max_frequency)
+		s->clock_freq = -servo->max_frequency;
+
+	return -s->clock_freq;
+}
+
+static void linreg_sync_interval(struct servo *servo, double interval)
+{
+	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+
+	s->update_interval = interval;
+}
+
+static void linreg_reset(struct servo *servo)
+{
+	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+	unsigned int i;
+
+	s->num_points = 0;
+	s->last_update = 0;
+
+	for (i = MIN_SIZE; i < MAX_SIZE; i++) {
+		s->results[i - MIN_SIZE].slope = 0.0;
+		s->results[i - MIN_SIZE].err_updates = 0;
+	}
+}
+
+struct servo *linreg_servo_create(int fadj)
+{
+	struct linreg_servo *s;
+
+	s = calloc(1, sizeof(*s));
+	if (!s)
+		return NULL;
+
+	s->servo.destroy = linreg_destroy;
+	s->servo.sample = linreg_sample;
+	s->servo.sync_interval = linreg_sync_interval;
+	s->servo.reset = linreg_reset;
+
+	s->clock_freq = -fadj;
+
+	return &s->servo;
+}

linreg.h

@@ -0,0 +1,26 @@
+/**
+ * @file linreg.h
+ * @note Copyright (C) 2014 Miroslav Lichvar <mlichvar@redhat.com>
+ *
+ * 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.
+ */
+#ifndef HAVE_LINREG_H
+#define HAVE_LINREG_H
+
+#include "servo.h"
+
+struct servo *linreg_servo_create(int fadj);
+
+#endif

makefile

@@ -24,7 +24,7 @@ CFLAGS	= -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
 LDLIBS	= -lm -lrt $(EXTRA_LDFLAGS)
 PRG	= ptp4l pmc phc2sys hwstamp_ctl
 OBJ     = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
- filter.o fsm.o mave.o mmedian.o msg.o phc.o pi.o port.o print.o ptp4l.o raw.o \
+ filter.o fsm.o linreg.o mave.o mmedian.o msg.o phc.o pi.o port.o print.o ptp4l.o raw.o \
  servo.o sk.o stats.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o
 
 OBJECTS	= $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o
@@ -47,9 +47,9 @@ ptp4l: $(OBJ)
 pmc: msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o transport.o udp.o \
  udp6.o uds.o util.o version.o
 
-phc2sys: clockadj.o clockcheck.o msg.o phc.o phc2sys.o pi.o pmc_common.o \
+phc2sys: clockadj.o clockcheck.o linreg.o msg.o phc.o phc2sys.o pi.o \
- print.o raw.o servo.o sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o \
+ pmc_common.o print.o raw.o servo.o sk.o stats.o sysoff.o tlv.o \
- uds.o util.o version.o
+ transport.o udp.o udp6.o uds.o util.o version.o
 
 hwstamp_ctl: hwstamp_ctl.o version.o
 

phc2sys.8

@@ -15,6 +15,8 @@ phc2sys \- synchronize two clocks
 ] [
 .BI \-O " offset"
 ] [
+.BI \-E " servo"
+] [
 .BI \-P " kp"
 ] [
 .BI \-I " ki"
@@ -89,6 +91,11 @@ should no longer be used.
 Specify the slave clock by device (e.g. /dev/ptp1) or interface (e.g. eth1) or
 by  name. The default is CLOCK_REALTIME (the system clock).
 .TP
+.BI \-E " servo"
+Specify which clock servo should be used. Valid values are pi for a PI
+controller and linreg for an adaptive controller using linear regression.
+The default is pi.
+.TP
 .BI \-P " kp"
 Specify the proportional constant of the PI controller. The default is 0.7.
 .TP

phc2sys.c

@@ -561,6 +561,7 @@ static void usage(char *progname)
 		" -c [dev|name]  slave clock (CLOCK_REALTIME)\n"
 		" -d [dev]       master PPS device\n"
 		" -s [dev|name]  master clock\n"
+		" -E [pi|linreg] clock servo (pi)\n"
 		" -P [kp]        proportional constant (0.7)\n"
 		" -I [ki]        integration constant (0.3)\n"
 		" -S [step]      step threshold (disabled)\n"
@@ -590,6 +591,7 @@ int main(int argc, char *argv[])
 	int max_ppb, r, wait_sync = 0, forced_sync_offset = 0;
 	int print_level = LOG_INFO, use_syslog = 1, verbose = 0;
 	int sanity_freq_limit = 200000000;
+	enum servo_type servo = CLOCK_SERVO_PI;
 	double ppb, phc_interval = 1.0, phc_rate;
 	struct timespec phc_interval_tp;
 	struct clock dst_clock = {
@@ -605,7 +607,7 @@ int main(int argc, char *argv[])
 	progname = strrchr(argv[0], '/');
 	progname = progname ? 1+progname : argv[0];
 	while (EOF != (c = getopt(argc, argv,
-				  "c:d:s:P:I:S:F:R:N:O:L:i:u:wn:xl:mqvh"))) {
+				  "c:d:s:E:P:I:S:F:R:N:O:L:i:u:wn:xl:mqvh"))) {
 		switch (c) {
 		case 'c':
 			dst_clock.clkid = clock_open(optarg);
@@ -624,6 +626,17 @@ int main(int argc, char *argv[])
 		case 's':
 			src = clock_open(optarg);
 			break;
+		case 'E':
+			if (!strcasecmp(optarg, "pi")) {
+				servo = CLOCK_SERVO_PI;
+			} else if (!strcasecmp(optarg, "linreg")) {
+				servo = CLOCK_SERVO_LINREG;
+			} else {
+				fprintf(stderr,
+					"invalid servo name %s\n", optarg);
+				return -1;
+			}
+			break;
 		case 'P':
 			if (get_arg_val_d(c, optarg, &configured_pi_kp,
 					  0.0, DBL_MAX))
@@ -795,7 +808,7 @@ int main(int argc, char *argv[])
 		}
 	}
 
-	dst_clock.servo = servo_create(CLOCK_SERVO_PI, -ppb, max_ppb, 0);
+	dst_clock.servo = servo_create(servo, -ppb, max_ppb, 0);
 
 	if (pps_fd >= 0) {
 		servo_sync_interval(dst_clock.servo, 1.0);

ptp4l.8

@@ -288,8 +288,8 @@ generated by the master.
 The default is 0 (disabled).
 .TP
 .B clock_servo
-The servo which is used to synchronize the local clock. Currently only one
+The servo which is used to synchronize the local clock. Valid values are pi for
-servo is implemented, a PI controller.
+a PI controller and linreg for an adaptive controller using linear regression.
 The default is pi.
 .TP
 .B pi_proportional_const

servo.c

@@ -18,6 +18,7 @@
  */
 #include <string.h>
 
+#include "linreg.h"
 #include "pi.h"
 #include "servo_private.h"
 
@@ -35,6 +36,9 @@ struct servo *servo_create(enum servo_type type, int fadj, int max_ppb, int sw_t
 	case CLOCK_SERVO_PI:
 		servo = pi_servo_create(fadj, sw_ts);
 		break;
+	case CLOCK_SERVO_LINREG:
+		servo = linreg_servo_create(fadj);
+		break;
 	default:
 		return NULL;
 	}

servo.h

@@ -54,6 +54,7 @@ struct servo;
  */
 enum servo_type {
 	CLOCK_SERVO_PI,
+	CLOCK_SERVO_LINREG,
 };
 
 /**