From 115ed4a44fa24f539bb5352d534fe124de58aaec Mon Sep 17 00:00:00 2001
From: Pei Mu <ds1231h@126.com>
Date: Wed, 15 Mar 2023 16:37:08 +0000
Subject: [PATCH] first version of kalman locally

Addresses #647.
---
 ...00a1cb296b4e0754792e2aa72d5af5940992c6.txt | 46 +++++++++++++
 .../newton/llvm-ir/c-files/kalman_filter.c    |  3 +
 applications/newton/llvm-ir/kalman_filter.c   | 66 +++++++++++++++++++
 3 files changed, 115 insertions(+)
 create mode 100644 analysis/statistics/a200a1cb296b4e0754792e2aa72d5af5940992c6.txt
 create mode 100644 applications/newton/llvm-ir/c-files/kalman_filter.c
 create mode 100644 applications/newton/llvm-ir/kalman_filter.c

diff --git a/analysis/statistics/a200a1cb296b4e0754792e2aa72d5af5940992c6.txt b/analysis/statistics/a200a1cb296b4e0754792e2aa72d5af5940992c6.txt
new file mode 100644
index 000000000..723419877
--- /dev/null
+++ b/analysis/statistics/a200a1cb296b4e0754792e2aa72d5af5940992c6.txt
@@ -0,0 +1,46 @@
+
+changeset: 1458:a200a1cb296b4e0754792e2aa72d5af5940992c6
+char kNewtonVersion[] = "0.3-alpha-1458 (a200a1cb296b4e0754792e2aa72d5af5940992c6) (build 03-15-2023-14:34-pei@pei-G5-5500-Linux-5.19.0-35-generic-x86_64)";
+\n./src/noisy/noisy-linux-EN -O0 applications/noisy/helloWorld.n -s
+\n./src/newton/newton-linux-EN -v 0 -eP applications/newton/invariants/ViolinWithTemperatureDependence-pigroups.nt
+
+Informational Report:
+---------------------
+Invariant "ViolinWithTemperatureDependenceForPiGroups" has 2 unique kernels, each with 2 column(s)...
+
+	Kernel 0 is a valid kernel:
+
+		   1   1
+		-0.5  -0
+		   1   0
+		 0.5   0
+		   0  -1
+		  -0  -1
+
+
+		The ordering of parameters is:	P1 P0 P3 P2 P4 P5 
+
+			Pi group 0, Pi 0 is:	P0^(-0.5)  P1^( 1)  P2^(0.5)  P3^( 1)  P4^( 0)  P5^(-0)  
+
+			Pi group 0, Pi 1 is:	P0^(-0)  P1^( 1)  P2^( 0)  P3^( 0)  P4^(-1)  P5^(-1)  
+
+
+	Kernel 1 is a valid kernel:
+
+		   1   0
+		-0.5   1
+		   1  -2
+		 0.5  -1
+		  -0  -2
+		   0  -2
+
+
+		The ordering of parameters is:	P1 P0 P3 P2 P4 P5 
+
+			Pi group 1, Pi 0 is:	P0^(-0.5)  P1^( 1)  P2^(0.5)  P3^( 1)  P4^(-0)  P5^( 0)  
+
+			Pi group 1, Pi 1 is:	P0^( 1)  P1^( 0)  P2^(-1)  P3^(-2)  P4^(-2)  P5^(-2)  
+
+
+
+
diff --git a/applications/newton/llvm-ir/c-files/kalman_filter.c b/applications/newton/llvm-ir/c-files/kalman_filter.c
new file mode 100644
index 000000000..a0baa5178
--- /dev/null
+++ b/applications/newton/llvm-ir/c-files/kalman_filter.c
@@ -0,0 +1,3 @@
+//
+// Created by pei on 15/03/23.
+//
diff --git a/applications/newton/llvm-ir/kalman_filter.c b/applications/newton/llvm-ir/kalman_filter.c
new file mode 100644
index 000000000..7ac21566f
--- /dev/null
+++ b/applications/newton/llvm-ir/kalman_filter.c
@@ -0,0 +1,66 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#define DT 0.01    // time step
+#define ACC_VAR 0.1 // acceleration measurement variance
+#define GYRO_VAR 0.1 // gyro measurement variance
+#define Q_ANGLE 0.1  // process noise covariance for the angle
+#define Q_BIAS 0.01  // process noise covariance for the gyro bias
+#define R_ANGLE 0.1  // measurement noise covariance for the angle
+
+typedef struct {
+    double angle;
+    double bias;
+    double rate;
+    double P[2][2];
+} kalman_state;
+
+// update the state of the Kalman filter based on a new measurement
+void kalman_update(kalman_state *state, double acc, double gyro) {
+    double y, S;
+    double K[2];
+
+    // update angle and bias estimates using the gyro measurement
+    state->angle += DT * (gyro - state->bias);
+    state->bias += Q_BIAS * DT;
+
+    // calculate the innovation and innovation covariance
+    y = acc - state->angle;
+    S = state->P[0][0] + R_ANGLE;
+
+    // calculate the Kalman gain
+    K[0] = state->P[0][0] / S;
+    K[1] = state->P[1][0] / S;
+
+    // update the state estimate and covariance
+    state->angle += K[0] * y;
+    state->bias += K[1] * y;
+    state->P[0][0] -= K[0] * state->P[0][0];
+    state->P[0][1] -= K[0] * state->P[0][1];
+    state->P[1][0] -= K[1] * state->P[0][0];
+    state->P[1][1] -= K[1] * state->P[0][1];
+
+    // propagate the state estimate and covariance using the gyro measurement
+    state->rate = gyro - state->bias;
+    state->angle += DT * state->rate;
+    state->P[0][0] += DT * (DT*state->P[1][1] - state->P[0][1] - state->P[1][0] + Q_ANGLE);
+    state->P[0][1] -= DT * state->P[1][1];
+    state->P[1][0] -= DT * state->P[1][1];
+    state->P[1][1] += Q_ANGLE * DT;
+}
+
+int main() {
+    double acc, gyro;
+    kalman_state state = {0, 0, 0, {{0, 0}, {0, 0}}};
+
+    // simulate some sensor measurements
+    for (int i = 0; i < 1000; i++) {
+        acc = 1.0 + ACC_VAR * (rand() / (double)RAND_MAX - 0.5);
+        gyro = 0.1 + GYRO_VAR * (rand() / (double)RAND_MAX - 0.5);
+        kalman_update(&state, acc, gyro);
+        printf("%f %f\n", state.angle, state.rate);
+    }
+
+    return 0;
+}
\ No newline at end of file