<p class="hint">An inverted pendulum on a cart: unstable on its own, it falls the instant you let go. The controller below computes a state-feedback gain <b>K</b> by solving the Riccati equation for your chosen cost weights, then applies force <code>u = -Kx</code> at every instant.</p>
<canvas id="scene" width="460" height="220"></canvas>
<div class="status" id="status">Ready. Press Compute LQR gain to balance the pole.</div>
<div class="controls">
<label>Angle error weight (Q) <span id="qVal">10</span>
<input id="qSlider" type="range" min="1" max="300" value="10" step="1">
</label>
<label>Control effort weight (R) <span id="rVal">1</span>
<input id="rSlider" type="range" min="1" max="50" value="1" step="1">
</label>
</div>
<div class="btns">
<button id="solve" type="button">Compute LQR gain</button>
<button id="nudge" type="button">Nudge the pole</button>
<button id="reset" type="button" class="ghost">Reset</button>
</div>
* { box-sizing: border-box; }
body { font-family: system-ui, sans-serif; color: #222; margin: 0; }
.hint { font-size: .9rem; color: #444; margin: 0 0 .6rem; line-height: 1.45; }
canvas { display: block; background: #eef2f6; border: 1px solid #cdd9e3; border-radius: 8px;
width: 100%; max-width: 460px; height: auto; }
.controls { display: flex; flex-wrap: wrap; gap: 1.2rem; margin: .6rem 0; }
.controls label { font-size: .85rem; font-weight: 600; color: #1d3557; display: flex;
flex-direction: column; gap: .2rem; min-width: 180px; }
.controls input[type="range"] { width: 180px; }
.status { font-size: 1rem; font-weight: 600; margin: .5rem 0; min-height: 1.4em; }
.status.ok { color: #0a7d33; }
.status.bad { color: #c92f3c; }
.btns { display: flex; gap: .5rem; flex-wrap: wrap; }
button { font: 600 14px system-ui, sans-serif; padding: .45rem .9rem; border: 1px solid #1d3557;
background: #1d3557; color: #fff; border-radius: 8px; cursor: pointer; }
button.ghost { background: #fff; color: #1d3557; }
const i18nFmt = (s, v) => s.replace(/%(\w+)%/g, (_, k) => String(v[k] ?? ''));
// Linearized cart-pole dynamics around the upright equilibrium (small-angle model).
// State x = [cart position, cart velocity, pole angle, angle rate]; input u = force on the cart.
const M = 1, m = 0.1, l = 0.5, g = 9.8;
const A = [[0,1,0,0],[0,0,(m*g)/M,0],[0,0,0,1],[0,0,((M+m)*g)/(M*l),0]];
const B = [[0],[1/M],[0],[1/(M*l)]];
// Small dense-matrix helpers (multiply, transpose, add, scale) — good enough for 4x4.
function mm(X,Y){ const r=X.length,k=X[0].length,c=Y[0].length; const o=Array.from({length:r},()=>Array(c).fill(0));
for(let i=0;i<r;i++)for(let j=0;j<c;j++)for(let p=0;p<k;p++) o[i][j]+=X[i][p]*Y[p][j]; return o; }
function tr(X){ return X[0].map((_,j)=>X.map(row=>row[j])); }
function add(X,Y,s=1){ return X.map((row,i)=>row.map((v,j)=>v+s*Y[i][j])); }
function sm(X,s){ return X.map(row=>row.map(v=>v*s)); }
// Solve the algebraic Riccati equation A^T P + P A - P B R^-1 B^T P + Q = 0
// by integrating the matrix differential Riccati equation forward until P stops changing (Kleinman-style relaxation).
function solveGain(qTheta, r){
const Q = [[1,0,0,0],[0,1,0,0],[0,0,qTheta,0],[0,0,0,1]];
const R = [[r]];
const n = 4;
let P = Array.from({length:n},(_,i)=>Array.from({length:n},(_,j)=> i===j?1:0));
const At = tr(A), Bt = tr(B), Rinv = [[1/R[0][0]]];
for(let it=0; it<4000; it++){
const PBRinvBtP = mm(mm(mm(P,B),Rinv), mm(Bt,P));
const dP = add(add(mm(At,P), mm(P,A)), add(sm(PBRinvBtP,-1), Q));
P = add(P, sm(dP, 0.001));
}
return mm(mm(Rinv, Bt), P)[0]; // K = R^-1 B^T P, returned as a single row (u is scalar here).
}
let K = solveGain(10, 1); // Initial gain uses default weights (Q_angle=10, R=1) so the pole starts stabilized.
let state = [0, 0, 0.3, 0]; // Start slightly tilted (0.3 rad) so you can watch the controller catch it.
let running = true;
let lastQ = 10, lastR = 1;
const qSlider = document.getElementById('qSlider');
const rSlider = document.getElementById('rSlider');
const qVal = document.getElementById('qVal');
const rVal = document.getElementById('rVal');
qSlider.addEventListener('input', () => { qVal.textContent = qSlider.value; });
rSlider.addEventListener('input', () => { rVal.textContent = rSlider.value; });
function setStatus(t, cls){ const s = document.getElementById('status'); s.textContent = t; s.className = 'status ' + cls; }
function step(dt){
// Closed-loop dynamics: apply u = -Kx, then integrate one Euler step of x' = Ax + Bu.
const u = -(K[0]*state[0] + K[1]*state[1] + K[2]*state[2] + K[3]*state[3]);
const dx = [ state[1], (m*g*state[2])/M + u/M, state[3], ((M+m)*g*state[2])/(M*l) + u/(M*l) ];
state = state.map((v,i) => v + dx[i]*dt);
if (Math.abs(state[2]) > Math.PI/2) { state[2] = Math.sign(state[2]) * Math.PI/2; running = false; }
}
function draw(){
const cv = document.getElementById('scene'); const ctx = cv.getContext('2d');
ctx.clearRect(0,0,cv.width,cv.height);
const originX = cv.width/2 + state[0]*60, groundY = 170;
ctx.strokeStyle = '#adb1b8'; ctx.beginPath(); ctx.moveTo(0,groundY); ctx.lineTo(cv.width,groundY); ctx.stroke();
ctx.fillStyle = '#1d3557'; ctx.fillRect(originX-25, groundY-15, 50, 30);
const poleLen = 130;
const tipX = originX + Math.sin(state[2]) * poleLen;
const tipY = groundY - 15 - Math.cos(state[2]) * poleLen;
ctx.strokeStyle = '#e63946'; ctx.lineWidth = 6; ctx.lineCap = 'round';
ctx.beginPath(); ctx.moveTo(originX, groundY-15); ctx.lineTo(tipX, tipY); ctx.stroke();
ctx.fillStyle = '#e63946'; ctx.beginPath(); ctx.arc(tipX, tipY, 8, 0, Math.PI*2); ctx.fill();
ctx.lineWidth = 1;
}
let rafId = null;
function loop(){
if (running) { step(0.016); draw(); }
if (running && Math.abs(state[2]) < 0.01 && Math.abs(state[3]) < 0.01) {
setStatus('✓ Balanced — angle and angular rate near zero.', 'ok');
} else if (!running) {
setStatus('✗ The pole fell. Reset and try different weights.', 'bad');
} else {
setStatus(i18nFmt('Angle: %deg%°', { deg: (state[2]*180/Math.PI).toFixed(1) }), '');
}
rafId = requestAnimationFrame(loop);
}
document.getElementById('solve').addEventListener('click', () => {
const qTheta = Number(qSlider.value), r = Number(rSlider.value);
K = solveGain(qTheta, r);
lastQ = qTheta; lastR = r;
running = true;
console.log('New LQR gain K =', K.map(v => v.toFixed(3)).join(', '));
setStatus(i18nFmt('Solved the Riccati equation for Q_angle=%q%, R=%r%. New gain applied.', { q: qTheta, r: r }), 'ok');
});
document.getElementById('nudge').addEventListener('click', () => {
state[2] += (Math.random() > 0.5 ? 1 : -1) * 0.25; // Give the pole a random push to test how well the current gain rejects disturbances.
running = true;
});
document.getElementById('reset').addEventListener('click', () => {
state = [0,0,0.3,0]; running = true; setStatus('Ready. Press Compute LQR gain to balance the pole.', '');
});
draw();
loop();
<p class="hint">An inverted pendulum on a cart: unstable on its own, it falls the instant you let go. The controller below computes a state-feedback gain <b>K</b> by solving the Riccati equation for your chosen cost weights, then applies force <code>u = -Kx</code> at every instant.</p>
<canvas id="scene" width="460" height="220"></canvas>
<div class="status" id="status">Ready. Press Compute LQR gain to balance the pole.</div>
<div class="controls">
<label>Angle error weight (Q) <span id="qVal">10</span>
<input id="qSlider" type="range" min="1" max="300" value="10" step="1">
</label>
<label>Control effort weight (R) <span id="rVal">1</span>
<input id="rSlider" type="range" min="1" max="50" value="1" step="1">
</label>
</div>
<div class="btns">
<button id="solve" type="button">Compute LQR gain</button>
<button id="nudge" type="button">Nudge the pole</button>
<button id="reset" type="button" class="ghost">Reset</button>
</div>
* { box-sizing: border-box; }
body { font-family: system-ui, sans-serif; color: #222; margin: 0; }
.hint { font-size: .9rem; color: #444; margin: 0 0 .6rem; line-height: 1.45; }
canvas { display: block; background: #eef2f6; border: 1px solid #cdd9e3; border-radius: 8px;
width: 100%; max-width: 460px; height: auto; }
.controls { display: flex; flex-wrap: wrap; gap: 1.2rem; margin: .6rem 0; }
.controls label { font-size: .85rem; font-weight: 600; color: #1d3557; display: flex;
flex-direction: column; gap: .2rem; min-width: 180px; }
.controls input[type="range"] { width: 180px; }
.status { font-size: 1rem; font-weight: 600; margin: .5rem 0; min-height: 1.4em; }
.status.ok { color: #0a7d33; }
.status.bad { color: #c92f3c; }
.btns { display: flex; gap: .5rem; flex-wrap: wrap; }
button { font: 600 14px system-ui, sans-serif; padding: .45rem .9rem; border: 1px solid #1d3557;
background: #1d3557; color: #fff; border-radius: 8px; cursor: pointer; }
button.ghost { background: #fff; color: #1d3557; }
const i18nFmt = (s, v) => s.replace(/%(\w+)%/g, (_, k) => String(v[k] ?? ''));
// Linearized cart-pole dynamics around the upright equilibrium (small-angle model).
// State x = [cart position, cart velocity, pole angle, angle rate]; input u = force on the cart.
const M = 1, m = 0.1, l = 0.5, g = 9.8;
const A = [[0,1,0,0],[0,0,(m*g)/M,0],[0,0,0,1],[0,0,((M+m)*g)/(M*l),0]];
const B = [[0],[1/M],[0],[1/(M*l)]];
// Small dense-matrix helpers (multiply, transpose, add, scale) — good enough for 4x4.
function mm(X,Y){ const r=X.length,k=X[0].length,c=Y[0].length; const o=Array.from({length:r},()=>Array(c).fill(0));
for(let i=0;i<r;i++)for(let j=0;j<c;j++)for(let p=0;p<k;p++) o[i][j]+=X[i][p]*Y[p][j]; return o; }
function tr(X){ return X[0].map((_,j)=>X.map(row=>row[j])); }
function add(X,Y,s=1){ return X.map((row,i)=>row.map((v,j)=>v+s*Y[i][j])); }
function sm(X,s){ return X.map(row=>row.map(v=>v*s)); }
// Solve the algebraic Riccati equation A^T P + P A - P B R^-1 B^T P + Q = 0
// by integrating the matrix differential Riccati equation forward until P stops changing (Kleinman-style relaxation).
function solveGain(qTheta, r){
const Q = [[1,0,0,0],[0,1,0,0],[0,0,qTheta,0],[0,0,0,1]];
const R = [[r]];
const n = 4;
let P = Array.from({length:n},(_,i)=>Array.from({length:n},(_,j)=> i===j?1:0));
const At = tr(A), Bt = tr(B), Rinv = [[1/R[0][0]]];
for(let it=0; it<4000; it++){
const PBRinvBtP = mm(mm(mm(P,B),Rinv), mm(Bt,P));
const dP = add(add(mm(At,P), mm(P,A)), add(sm(PBRinvBtP,-1), Q));
P = add(P, sm(dP, 0.001));
}
return mm(mm(Rinv, Bt), P)[0]; // K = R^-1 B^T P, returned as a single row (u is scalar here).
}
let K = solveGain(10, 1); // Initial gain uses default weights (Q_angle=10, R=1) so the pole starts stabilized.
let state = [0, 0, 0.3, 0]; // Start slightly tilted (0.3 rad) so you can watch the controller catch it.
let running = true;
let lastQ = 10, lastR = 1;
const qSlider = document.getElementById('qSlider');
const rSlider = document.getElementById('rSlider');
const qVal = document.getElementById('qVal');
const rVal = document.getElementById('rVal');
qSlider.addEventListener('input', () => { qVal.textContent = qSlider.value; });
rSlider.addEventListener('input', () => { rVal.textContent = rSlider.value; });
function setStatus(t, cls){ const s = document.getElementById('status'); s.textContent = t; s.className = 'status ' + cls; }
function step(dt){
// Closed-loop dynamics: apply u = -Kx, then integrate one Euler step of x' = Ax + Bu.
const u = -(K[0]*state[0] + K[1]*state[1] + K[2]*state[2] + K[3]*state[3]);
const dx = [ state[1], (m*g*state[2])/M + u/M, state[3], ((M+m)*g*state[2])/(M*l) + u/(M*l) ];
state = state.map((v,i) => v + dx[i]*dt);
if (Math.abs(state[2]) > Math.PI/2) { state[2] = Math.sign(state[2]) * Math.PI/2; running = false; }
}
function draw(){
const cv = document.getElementById('scene'); const ctx = cv.getContext('2d');
ctx.clearRect(0,0,cv.width,cv.height);
const originX = cv.width/2 + state[0]*60, groundY = 170;
ctx.strokeStyle = '#adb1b8'; ctx.beginPath(); ctx.moveTo(0,groundY); ctx.lineTo(cv.width,groundY); ctx.stroke();
ctx.fillStyle = '#1d3557'; ctx.fillRect(originX-25, groundY-15, 50, 30);
const poleLen = 130;
const tipX = originX + Math.sin(state[2]) * poleLen;
const tipY = groundY - 15 - Math.cos(state[2]) * poleLen;
ctx.strokeStyle = '#e63946'; ctx.lineWidth = 6; ctx.lineCap = 'round';
ctx.beginPath(); ctx.moveTo(originX, groundY-15); ctx.lineTo(tipX, tipY); ctx.stroke();
ctx.fillStyle = '#e63946'; ctx.beginPath(); ctx.arc(tipX, tipY, 8, 0, Math.PI*2); ctx.fill();
ctx.lineWidth = 1;
}
let rafId = null;
function loop(){
if (running) { step(0.016); draw(); }
if (running && Math.abs(state[2]) < 0.01 && Math.abs(state[3]) < 0.01) {
setStatus('✓ Balanced — angle and angular rate near zero.', 'ok');
} else if (!running) {
setStatus('✗ The pole fell. Reset and try different weights.', 'bad');
} else {
setStatus(i18nFmt('Angle: %deg%°', { deg: (state[2]*180/Math.PI).toFixed(1) }), '');
}
rafId = requestAnimationFrame(loop);
}
document.getElementById('solve').addEventListener('click', () => {
const qTheta = Number(qSlider.value), r = Number(rSlider.value);
K = solveGain(qTheta, r);
lastQ = qTheta; lastR = r;
running = true;
console.log('New LQR gain K =', K.map(v => v.toFixed(3)).join(', '));
setStatus(i18nFmt('Solved the Riccati equation for Q_angle=%q%, R=%r%. New gain applied.', { q: qTheta, r: r }), 'ok');
});
document.getElementById('nudge').addEventListener('click', () => {
state[2] += (Math.random() > 0.5 ? 1 : -1) * 0.25; // Give the pole a random push to test how well the current gain rejects disturbances.
running = true;
});
document.getElementById('reset').addEventListener('click', () => {
state = [0,0,0.3,0]; running = true; setStatus('Ready. Press Compute LQR gain to balance the pole.', '');
});
draw();
loop();
Comments
Loading comments...