From 9e7c732afdf570037390b1d36fbaac690671beac Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Mon, 23 Mar 2026 12:10:20 +0300 Subject: [PATCH] kalman added --- PID.c | 1 + kalman.c | 169 ++++++++++++++++++++++++++++++++++++++++++++++ kalman.h | 34 ++++++++++ libsidservo.files | 2 + serial.c | 126 ++++++++++++---------------------- sidservo.h | 1 + ssii.c | 29 +++++--- 7 files changed, 270 insertions(+), 92 deletions(-) create mode 100644 kalman.c create mode 100644 kalman.h diff --git a/PID.c b/PID.c index 0178c3e..6084a0a 100644 --- a/PID.c +++ b/PID.c @@ -149,6 +149,7 @@ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_ // TODO: we can point somehow that we are at target or introduce new axis state }else DBG("Current abs error: %g", fe); break; + case AXIS_GONNASTOP: case AXIS_STOPPED: // start pointing to target; will change speed next time DBG("AXIS STOPPED!!!! --> Slewing"); axis->state = AXIS_SLEWING; diff --git a/kalman.c b/kalman.c new file mode 100644 index 0000000..0c569bd --- /dev/null +++ b/kalman.c @@ -0,0 +1,169 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * 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 3 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, see . + */ + +#include + +#include "kalman.h" + +void kalman3_init(Kalman3 *kf, double dt, double enc_var){ + kf->dt = dt; + + kf->x[0] = 0; + kf->x[1] = 0; + kf->x[2] = 0; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + kf->P[i][j] = 0; + + kf->P[0][0] = 1; + kf->P[1][1] = 1; + kf->P[2][2] = 1; + + // process noise + kf->Q[0][0] = 1e-6; + kf->Q[1][1] = 1e-4; + kf->Q[2][2] = 1e-3; + + kf->R = enc_var; // encoder noise variance +} + +void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j){ + double dt = kf->dt; + + double dt2 = dt*dt; + double dt3 = dt2*dt; + double dt4 = dt3*dt; + double dt5 = dt4*dt; + + double q = sigma_j * sigma_j; + + kf->Q[0][0] = q * dt5 / 20.0; + kf->Q[0][1] = q * dt4 / 8.0; + kf->Q[0][2] = q * dt3 / 6.0; + + kf->Q[1][0] = q * dt4 / 8.0; + kf->Q[1][1] = q * dt3 / 3.0; + kf->Q[1][2] = q * dt2 / 2.0; + + kf->Q[2][0] = q * dt3 / 6.0; + kf->Q[2][1] = q * dt2 / 2.0; + kf->Q[2][2] = q * dt; +} + +void kalman3_predict(Kalman3 *kf){ + double dt = kf->dt; + double dt2 = 0.5 * dt * dt; + + double theta = kf->x[0]; + double omega = kf->x[1]; + double alpha = kf->x[2]; + + // state prediction + kf->x[0] = theta + omega*dt + alpha*dt2; + kf->x[1] = omega + alpha*dt; + kf->x[2] = alpha; + + // transition matrix + double F[3][3] = + { + {1, dt, dt2}, + {0, 1, dt}, + {0, 0, 1} + }; + + // P = FPF^T + Q + + double FP[3][3]; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++){ + FP[i][j] = + F[i][0]*kf->P[0][j] + + F[i][1]*kf->P[1][j] + + F[i][2]*kf->P[2][j]; + } + + double Pnew[3][3]; + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++){ + Pnew[i][j] = + FP[i][0]*F[j][0] + + FP[i][1]*F[j][1] + + FP[i][2]*F[j][2] + + kf->Q[i][j]; + } + + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + kf->P[i][j] = Pnew[i][j]; +} + +void kalman3_update(Kalman3 *kf, double z){ + // innovation + double y = z - kf->x[0]; + + // S = HPH^T + R + double S = kf->P[0][0] + kf->R; + + // Kalman gain + double K[3]; + + K[0] = kf->P[0][0] / S; + K[1] = kf->P[1][0] / S; + K[2] = kf->P[2][0] / S; + + // state update + kf->x[0] += K[0] * y; + kf->x[1] += K[1] * y; + kf->x[2] += K[2] * y; + + // covariance update + double P00 = kf->P[0][0]; + double P01 = kf->P[0][1]; + double P02 = kf->P[0][2]; + + double P10 = kf->P[1][0]; + double P11 = kf->P[1][1]; + double P12 = kf->P[1][2]; + + double P20 = kf->P[2][0]; + double P21 = kf->P[2][1]; + double P22 = kf->P[2][2]; + + kf->P[0][0] = P00 - K[0]*P00; + kf->P[0][1] = P01 - K[0]*P01; + kf->P[0][2] = P02 - K[0]*P02; + + kf->P[1][0] = P10 - K[1]*P00; + kf->P[1][1] = P11 - K[1]*P01; + kf->P[1][2] = P12 - K[1]*P02; + + kf->P[2][0] = P20 - K[2]*P00; + kf->P[2][1] = P21 - K[2]*P01; + kf->P[2][2] = P22 - K[2]*P02; +} + + +// estimation of the R +double encoder_noise(int counts){ + double d = 2.0*M_PI / counts; + return d*d / 12.0; +} + diff --git a/kalman.h b/kalman.h new file mode 100644 index 0000000..f1aa7d8 --- /dev/null +++ b/kalman.h @@ -0,0 +1,34 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * 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 3 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, see . + */ + +#pragma once + +typedef struct{ + double x[3]; // [theta, omega, alpha] + double P[3][3]; // covariance + double Q[3][3]; // process noise + double R; // measurement noise + double dt; +} Kalman3; + + +double encoder_noise(int counts); +void kalman3_update(Kalman3 *kf, double z); +void kalman3_predict(Kalman3 *kf); +void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j); +void kalman3_init(Kalman3 *kf, double dt, double enc_var); diff --git a/libsidservo.files b/libsidservo.files index aa9288a..4e1eee2 100644 --- a/libsidservo.files +++ b/libsidservo.files @@ -13,12 +13,14 @@ examples/dumpswing.c examples/goto.c examples/scmd_traectory.c examples/simpleconv.h +kalman.c main.c sidservo.h serial.c examples/CMakeLists.txt examples/traectories.c examples/traectories.h +kalman.h main.h movingmodel.c movingmodel.h diff --git a/serial.c b/serial.c index 67e06b7..b8e4665 100644 --- a/serial.c +++ b/serial.c @@ -31,6 +31,7 @@ #include #include +#include "kalman.h" #include "main.h" #include "movingmodel.h" #include "serial.h" @@ -141,77 +142,6 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI); } -#if 0 -/** - * @brief getencval - get uint64_t data from encoder - * @param fd - encoder fd - * @param val - value read - * @param t - measurement time - * @return amount of data read or 0 if problem - */ -static int getencval(int fd, double *val, struct timespec *t){ - if(fd < 0){ - DBG("Encoder fd < 0!"); - return FALSE; - } - char buf[128]; - int got = 0, Lmax = 127; - double t0 = timefromstart(); - //DBG("start: %.6g", t0); - do{ - fd_set rfds; - FD_ZERO(&rfds); - FD_SET(fd, &rfds); - struct timeval tv = encRtmout; - int retval = select(fd + 1, &rfds, NULL, NULL, &tv); - if(!retval){ - //DBG("select()==0 - timeout, %.6g", timefromstart()); - break; - } - if(retval < 0){ - if(errno == EINTR){ - DBG("EINTR"); - continue; - } - DBG("select() < 0"); - return 0; - } - if(FD_ISSET(fd, &rfds)){ - ssize_t l = read(fd, &buf[got], Lmax); - if(l < 1){ - DBG("read() < 0"); - return 0; // disconnected ?? - } - got += l; Lmax -= l; - buf[got] = 0; - } else continue; - if(buf[got-1] == '\n') break; // got EOL as last symbol - }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.); - if(got == 0){ - //DBG("No data from encoder, tfs=%.6g", timefromstart()); - return 0; - } - char *estr = strrchr(buf, '\n'); - if(!estr){ - DBG("No EOL"); - return 0; - } - *estr = 0; - char *bgn = strrchr(buf, '\n'); - if(bgn) ++bgn; - else bgn = buf; - char *eptr; - long data = strtol(bgn, &eptr, 10); - if(eptr != estr){ - DBG("NAN"); - return 0; // wrong number - } - if(val) *val = (double) data; - if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} - return got; -} -#endif - // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected static int getencbyte(){ if(encfd[0] < 0) return -1; @@ -385,7 +315,7 @@ static int getdata(buf_t *buf, long *out){ if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){ return FALSE; } - DBG("got data"); +// DBG("got data"); // read record between last '\n' and previous (or start of string) char *last = &buf->buf[buf->len - 1]; //DBG("buf: _%s_", buf->buf); @@ -433,6 +363,18 @@ static void *encoderthread2(void _U_ *u){ asknext(encfd[0]); asknext(encfd[1]); t0[0] = t0[1] = tstart = timefromstart(); int errctr = 0; + + // init Kalman for both axes + Kalman3 kf[2]; + double dt = Conf.EncoderReqInterval; // 1ms encoders step + double sigma_jx = 1e-6, sigma_jy = 1e-6; // "jerk" sigma + double xnoice = encoder_noise(X_ENC_STEPSPERREV); + double ynoice = encoder_noise(Y_ENC_STEPSPERREV); + kalman3_init(&kf[0], dt, xnoice); + kalman3_init(&kf[1], dt, ynoice); + kalman3_set_jerk_noise(&kf[0], sigma_jx); + kalman3_set_jerk_noise(&kf[1], sigma_jy); + do{ // main cycle if(poll(pfds, 2, 0) < 0){ DBG("poll()"); @@ -450,25 +392,37 @@ static void *encoderthread2(void _U_ *u){ double curt = timefromstart(); if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt; if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records - DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]); + //DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]); if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ - DBG("time OK"); + //DBG("time OK"); pthread_mutex_lock(&datamutex); double pos = (double)msrlast[i]; - //DBG("pos[%d]=%g", i, pos); - ;; - //DBG("Got med: %g", pos); if(i == 0){ - mountdata.encXposition.val = Xenc2rad(pos); + pos = Xenc2rad(pos); + // Kalman filtering + kalman3_predict(&kf[i]); + kalman3_update(&kf[i], pos); + DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + mountdata.encXposition.val = kf[i].x[0]; curtime(&mountdata.encXposition.t); /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g", msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec, X_ENC_ZERO, X_ENC_STEPSPERREV);*/ - getXspeed(); + //getXspeed(); + mountdata.encXspeed.val = kf[i].x[1]; + mountdata.encXspeed.t = mountdata.encXposition.t; }else{ - mountdata.encYposition.val = Yenc2rad(pos); + pos = Yenc2rad(pos); + kalman3_predict(&kf[i]); + kalman3_update(&kf[i], pos); + DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g", + pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]); + mountdata.encYposition.val = kf[i].x[0]; curtime(&mountdata.encYposition.t); - getYspeed(); + //getYspeed(); + mountdata.encYspeed.val = kf[i].x[1]; + mountdata.encYspeed.t = mountdata.encYposition.t; } pthread_mutex_unlock(&datamutex); } @@ -860,11 +814,17 @@ static int bincmd(uint8_t *cmd, int len){ }else{ goto rtn; } - data_t d; + SSstat ans; + data_t d, in; d.buf = cmd; d.len = d.maxlen = len; - ret = wr(&d, NULL, 0); + in.buf = (uint8_t*)&ans; in.maxlen = sizeof(SSstat); + ret = wr(&d, &in, 0); DBG("%s", ret ? "SUCCESS" : "FAIL"); + if(ret){ + DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d", + ans.Xmot, ans.Ymot, ans.XLast, ans.YLast); + } rtn: pthread_mutex_unlock(&mntmutex); return ret; diff --git a/sidservo.h b/sidservo.h index e544ac1..ba4b8e5 100644 --- a/sidservo.h +++ b/sidservo.h @@ -137,6 +137,7 @@ typedef struct{ typedef enum{ AXIS_STOPPED, // stop + AXIS_GONNASTOP, // stop command run AXIS_SLEWING, // go to target with maximal speed AXIS_POINTING, // axis is in pointing zone, use PID AXIS_GUIDING, // near target diff --git a/ssii.c b/ssii.c index 76d9a0b..87ec05c 100644 --- a/ssii.c +++ b/ssii.c @@ -45,27 +45,35 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ } // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!! -static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){ +static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){ if(*prev == INT32_MAX){ - *stat = AXIS_STOPPED; + stat = AXIS_STOPPED; DBG("START"); - }else if(*stat != AXIS_STOPPED){ - if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){ - *stat = AXIS_STOPPED; - DBG("AXIS stopped"); - } + }else if(stat == AXIS_GONNASTOP){ // check stop + if(*prev == cur){ + if(++(*nstopped) > MOTOR_STOPPED_CNT){ + stat = AXIS_STOPPED; + DBG("AXIS stopped"); + } + }else *nstopped = 0; }else if(*prev != cur){ DBG("AXIS moving"); *nstopped = 0; } *prev = cur; + return stat; } // check for stopped/pointing states static void ChkStopped(const SSstat *s, mountdata_t *m){ static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state - chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); - chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate); + axis_status_t Xstat, Ystat; + Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate); + Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate); + if(Xstat != m->Xstate || Ystat != m->Ystate){ + DBG("Status changed"); + setStat(Xstat, Ystat); + } } /** @@ -170,14 +178,17 @@ int SSsetterI(const char *cmd, int32_t ival){ } int SSstop(int emerg){ + FNAME(); int i = 0; const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX; const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY; + setStat(AXIS_GONNASTOP, AXIS_GONNASTOP); for(; i < 10; ++i){ if(!SStextcmd(cmdx, NULL)) continue; if(SStextcmd(cmdy, NULL)) break; } if(i == 10) return FALSE; + DBG("Stopped"); return TRUE; }